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

RegistrationVis: fixed assert covariance in bundle adjustement when transform is perfect (covariance is null) by setting an epsilon to generated covariance is null or very close to 0

Browse Source
This commit is contained in:
matlabbe
2017-07-26 15:27:54 -04:00
parent be720be74a
commit 3443c3b8ea
2 changed files with 22 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Registration.cpp
View File

@@ -257,17 +257,17 @@ void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & t
} }
double epsilon = 0.000001; double epsilon = 0.000001;
if(covariance.at<double>(0,0)<=0.0) if(covariance.at<double>(0,0)<=epsilon)
covariance.at<double>(0,0) = epsilon; // epsilon if exact transform covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariance.at<double>(1,1)<=0.0) if(covariance.at<double>(1,1)<=epsilon)
covariance.at<double>(1,1) = epsilon; // epsilon if exact transform covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariance.at<double>(2,2)<=0.0) if(covariance.at<double>(2,2)<=epsilon)
covariance.at<double>(2,2) = epsilon; // epsilon if exact transform covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariance.at<double>(3,3)<=0.0) if(covariance.at<double>(3,3)<=epsilon)
covariance.at<double>(3,3) = epsilon; // epsilon if exact transform covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariance.at<double>(4,4)<=0.0) if(covariance.at<double>(4,4)<=epsilon)
covariance.at<double>(4,4) = epsilon; // epsilon if exact transform covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariance.at<double>(5,5)<=0.0) if(covariance.at<double>(5,5)<=epsilon)
covariance.at<double>(5,5) = epsilon; // epsilon if exact transform covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
} }

18
corelib/src/RegistrationVis.cpp
View File

@@ -1234,6 +1234,20 @@ Transform RegistrationVis::computeTransformationImpl(
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
double epsilon = 0.000001;
if(covariances[dir].at<double>(0,0)<=epsilon)
covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(1,1)<=epsilon)
covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(2,2)<=epsilon)
covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(3,3)<=epsilon)
covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(4,4)<=epsilon)
covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(5,5)<=epsilon)
covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
} }
if(!_forwardEstimateOnly) if(!_forwardEstimateOnly)
@@ -1292,13 +1306,13 @@ Transform RegistrationVis::computeTransformationImpl(
poses.insert(std::make_pair(2, transforms[0])); poses.insert(std::make_pair(2, transforms[0]));
cv::Mat cov = covariances[0].clone(); cv::Mat cov = covariances[0].clone();
normalizeCovariance(cov, transform); normalizeCovariance(cov, transforms[0]);
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv()))); links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
if(!transforms[1].isNull() && inliers[1].size()) if(!transforms[1].isNull() && inliers[1].size())
{ {
cov = covariances[1].clone(); cov = covariances[1].clone();
normalizeCovariance(cov, transform); normalizeCovariance(cov, transforms[1]);
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv()))); links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
} }