i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added util3d_motion_estimation.h tests (2D->3D done)

Browse Source
This commit is contained in:
matlabbe
2025-07-05 20:42:32 -07:00
parent 7a7c33dd14
commit 3c5c00456b
4 changed files with 578 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_motion_estimation.cpp
+14 -9
View File
@@ -224,11 +224,11 @@ Transform estimateMotion3DTo2D(
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin + 1e-6;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang + 1e-6;
if(splitLinearCovarianceComponents)
{
@@ -242,9 +242,9 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
covariance->at<double>(0,0) = median_error_sqr_x + 1e-6;
covariance->at<double>(1,1) = median_error_sqr_y + 1e-6;
covariance->at<double>(2,2) = median_error_sqr_z + 1e-6;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
@@ -267,7 +267,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);
}
UASSERT(uIsFinite(err));
*covariance *= std::sqrt(err/float(inliers.size()));
*covariance *= std::sqrt(err/float(inliers.size())) + 1e-6;
}
}
}
@@ -483,6 +483,7 @@ Transform estimateMotion3DTo2D(
// convert 2d-3d correspondences into bearing vectors
std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
multiBearingVectors.resize(cameraModels.size());
std::vector<std::vector<int> > localIndexToGlobalIndex(cameraModels.size());
for(size_t i=0; i<cameraModels.size();++i)
{
multiPoints[i] = std::make_shared<opengv::points_t>();
@@ -497,6 +498,7 @@ Transform estimateMotion3DTo2D(
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
localIndexToGlobalIndex[cameraIndex].push_back(i);
}
//create a non-central absolute multi adapter
@@ -527,9 +529,10 @@ Transform estimateMotion3DTo2D(
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
for (size_t i=0; i < cameraModels.size(); ++i)
{
inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
for (size_t i=0; i < cameraModels.size(); ++i) {
for (size_t j=0; j < ransac.inliers_[i].size(); ++j) {
inliers.push_back(localIndexToGlobalIndex[i][ransac.inliers_[i][j]]);
}
}
}
else
@@ -705,6 +708,7 @@ Transform estimateMotion3DTo2D(
if(matchesOut)
{
matchesOut->clear();
matchesOut->resize(cameraModels.size());
UASSERT(matches.size() == cameraIndexes.size());
for(size_t i=0; i<matches.size(); ++i)
@@ -715,6 +719,7 @@ Transform estimateMotion3DTo2D(
}
if(inliersOut)
{
inliersOut->clear();
inliersOut->resize(cameraModels.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{