i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

61
corelib/src/Registration.cpp
View File

@@ -212,35 +212,8 @@ Transform Registration::computeTransformationMod(
}
}
if(covarianceNormalized_)
{
// normalize variance
UASSERT(info.covariance.cols == 6 && info.covariance.rows == 6);
float norm = t.getNorm();
if(norm > 0.0f)
{
cv::Mat(info.covariance, cv::Range(0,3), cv::Range(0,3)) *= norm;
}
float angle = t.getAngle();
if(angle > 0.0f)
{
cv::Mat(info.covariance, cv::Range(3,6), cv::Range(3,6)) *= angle;
}
}
double epsilon = 0.000001;
if(info.covariance.at<double>(0,0)<=0.0)
info.covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=0.0)
info.covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=0.0)
info.covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=0.0)
info.covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=0.0)
info.covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=0.0)
info.covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
normalizeCovariance(info.covariance, t);
if(child_)
{
@@ -266,4 +239,36 @@ Transform Registration::computeTransformationMod(
return t;
}
void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & transform) const
{
UASSERT(covariance.cols == 6 && covariance.rows == 6);
if(covarianceNormalized_)
{
// normalize variance
float norm = transform.getNorm();
covariance.at<double>(0,0) *= norm;
covariance.at<double>(1,1) *= norm;
covariance.at<double>(2,2) *= norm;
float angle = transform.getAngle()/10.0;
covariance.at<double>(3,3) *= angle;
covariance.at<double>(4,4) *= angle;
covariance.at<double>(5,5) *= angle;
}
double epsilon = 0.000001;
if(covariance.at<double>(0,0)<=0.0)
covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariance.at<double>(1,1)<=0.0)
covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariance.at<double>(2,2)<=0.0)
covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariance.at<double>(3,3)<=0.0)
covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariance.at<double>(4,4)<=0.0)
covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariance.at<double>(5,5)<=0.0)
covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
}
}

21
corelib/src/RegistrationVis.cpp
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())));
}