i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-09 03:27:45 +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
corelib/src/Registration.cpp
+6 -6
View File
@@ -257,17 +257,17 @@ void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & t
}
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
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
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
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
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
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
}