i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.13.0 Database: Added velocity field to Node table, renamed Map_Node_Word table to Feature, added information_matrix field to Link table (replacing rot_variance and trans_variance). Added graph::calcKittiSequenceErrors().Added "Mem/IntermediateNodeDataKept" parameter (default false). Updated all interfaces using rotVariance and transVariance values with covariance matrix instead. g2o SBA: always use covariance in links. kitti-dataset tool: added --disp option and kitti statistics are shown at the end. StatsToolBox: units can have "/".

Browse Source
This commit is contained in:
matlabbe
2017-05-11 15:02:18 -04:00
parent 414e3555a5
commit 104c1e6945
45 changed files with 1539 additions and 1148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_motion_estimation.cpp
+11 -11
View File
@@ -56,7 +56,7 @@ Transform estimateMotion3DTo2D(
int refineIterations,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
double * varianceOut,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
@@ -65,9 +65,9 @@ Transform estimateMotion3DTo2D(
Transform transform;
std::vector<int> matches, inliers;
if(varianceOut)
if(covariance)
{
*varianceOut = 1.0;
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
// find correspondences
@@ -138,7 +138,7 @@ Transform estimateMotion3DTo2D(
transform = (cameraModel.localTransform() * pnp).inverse();
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(varianceOut && words3B.size())
if(covariance && words3B.size())
{
std::vector<float> errorSqrdDists(inliers.size());
oi = 0;
@@ -162,10 +162,10 @@ Transform estimateMotion3DTo2D(
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
*varianceOut = 2.1981 * median_error_sqr;
*covariance *= 2.1981 * median_error_sqr;
}
}
else if(varianceOut)
else if(covariance)
{
// compute variance, which is the rms of reprojection errors
std::vector<cv::Point2f> imagePointsReproj;
@@ -175,7 +175,7 @@ Transform estimateMotion3DTo2D(
{
err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
}
*varianceOut = std::sqrt(err/float(inliers.size()));
*covariance *= std::sqrt(err/float(inliers.size()));
}
}
}
@@ -203,7 +203,7 @@ Transform estimateMotion3DTo3D(
double inliersDistance,
int iterations,
int refineIterations,
double * varianceOut,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
@@ -222,9 +222,9 @@ Transform estimateMotion3DTo3D(
UASSERT(inliers1.size() == inliers2.size());
UDEBUG("Unique correspondences = %d", (int)inliers1.size());
if(varianceOut)
if(covariance)
{
*varianceOut = 1.0;
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
std::vector<int> inliers;
@@ -251,7 +251,7 @@ Transform estimateMotion3DTo3D(
refineIterations,
3.0,
&inliers,
varianceOut);
covariance);
if(!t.isNull() && (int)inliers.size() >= minInliers)
{