i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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/include/rtabmap/core/Registration.h
+2
View File
@@ -85,6 +85,8 @@ public:
Transform guess = Transform::getIdentity(), Transform guess = Transform::getIdentity(),
RegistrationInfo * info = 0) const; RegistrationInfo * info = 0) const;
void normalizeCovariance(cv::Mat & covariance, const Transform & transform) const;
protected: protected:
// take ownership of child // take ownership of child
Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0); Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
corelib/src/Registration.cpp
+33 -28
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; normalizeCovariance(info.covariance, t);
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
if(child_) if(child_)
{ {
@@ -266,4 +239,36 @@ Transform Registration::computeTransformationMod(
return t; 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
}
} }
corelib/src/RegistrationVis.cpp
+3 -18
View File
@@ -1292,28 +1292,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();
if(covarianceNormalized()) normalizeCovariance(cov, transform);
{
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
}
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();
if(covarianceNormalized()) normalizeCovariance(cov, transform);
{
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
}
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())));
} }