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

Reg: normalize covariance angle /10

Browse Source
This commit is contained in:
matlabbe
2017-05-22 21:11:36 -04:00
parent 385b7e3389
commit e361abcecd
3 changed files with 38 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RegistrationVis.cpp
+3 -18
View File
@@ -1292,28 +1292,13 @@ Transform RegistrationVis::computeTransformationImpl(
poses.insert(std::make_pair(2, transforms[0]));
cv::Mat cov = covariances[0].clone();
if(covarianceNormalized())
{
cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
}
if(cov.at<double>(0,0)<=0.0)
{
cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
}
normalizeCovariance(cov, transform);
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
if(!transforms[1].isNull() && inliers[1].size())
{
cov = covariances[1].clone();
if(covarianceNormalized())
{
cv::Mat(cov, cv::Range(0,3), cv::Range(0,3)) *= transform.getNorm();
cv::Mat(cov, cv::Range(3,6), cv::Range(3,6)) *= transform.getAngle();
}
if(cov.at<double>(0,0)<=0.0)
{
cov = cv::Mat::eye(6,6,CV_64FC1)*0.000001; // epsilon if exact transform
}
normalizeCovariance(cov, transform);
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
}