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

Parameters: Added "Reg/VarianceNormalized". Registration: Transforms are now only normalized in Registration. Added rgbd_dataset tool. MainWindow: odom inliers-only shown option. ExportCloudsDialog: ignoring intermediate nodes for texturing (if they don't have data). DatabaseViewer: "optimized" checkbox enabled in Constraints view for non-neighbor links too.

Browse Source
This commit is contained in:
matlabbe
2017-05-22 11:48:01 -04:00
parent 58e6424bf7
commit 385b7e3389
20 changed files with 758 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Link.cpp
View File

@@ -117,7 +117,7 @@ Link Link::merge(const Link & link, Type outputType) const
link.to(),
outputType,
transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
}
Link Link::inverse() const

19
corelib/src/Memory.cpp
View File

@@ -2269,15 +2269,6 @@ Transform Memory::computeTransform(
info->rejectedMsg = msg;
}
}
else if(info && !transform.isIdentity())
{
//normalize variance
info->covariance *= transform.getNorm();
if(info->covariance.at<double>(0,0) < 0.0001)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
}
}
return transform;
@@ -2325,16 +2316,6 @@ Transform Memory::computeIcpTransform(
// compute transform fromId -> toId
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
if(!t.isNull() && !t.isIdentity() && info)
{
// normalize variance
info->covariance *= t.getNorm();
if(info->covariance.at<double>(0,0)<=0.0)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
}
else
{

6
corelib/src/Odometry.cpp
View File

@@ -529,12 +529,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
info->distanceTravelled = distanceTravelled_;
}
info->covariance *= t.getNorm();
if(info->covariance.at<double>(0,0)<=0.0)
{
info->covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
return _pose *= t; // update
}
else if(_resetCurrentCount > 0)

1
corelib/src/OdometryF2M.cpp
View File

@@ -250,7 +250,6 @@ Transform OdometryF2M::computeTransform(
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
regInfo.covariance *= transform.getNorm();
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));

37
corelib/src/Registration.cpp
View File

@@ -62,6 +62,7 @@ Registration * Registration::create(Registration::Type & type, const ParametersM
Registration::Registration(const ParametersMap & parameters, Registration * child) :
varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
covarianceNormalized_(Parameters::defaultRegVarianceNormalized()),
force3DoF_(Parameters::defaultRegForce3DoF()),
child_(child)
{
@@ -78,6 +79,7 @@ Registration::~Registration()
void Registration::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
Parameters::parse(parameters, Parameters::kRegVarianceNormalized(), covarianceNormalized_);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
if(child_)
{
@@ -193,6 +195,11 @@ Transform Registration::computeTransformationMod(
Transform t = computeTransformationImpl(from, to, guess, info);
if(info.covariance.empty())
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
if(varianceFromInliersCount_)
{
if(info.icpInliersRatio)
@@ -203,12 +210,38 @@ Transform Registration::computeTransformationMod(
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
}
if(info.covariance.at<double>(0,0)<0.0001)
}
if(covarianceNormalized_)
{
// normalize variance
UASSERT(info.covariance.cols == 6 && info.covariance.rows == 6);
float norm = t.getNorm();
if(norm > 0.0f)
{
info.covariance =cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
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
if(child_)
{
if(!t.isNull())

26
corelib/src/RegistrationVis.cpp
View File

@@ -1291,10 +1291,30 @@ Transform RegistrationVis::computeTransformationImpl(
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, transforms[0]));
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], (covariances[0]*transforms[0].getNorm()).inv())));
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
}
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
if(!transforms[1].isNull() && inliers[1].size())
{
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], (covariances[1]*transforms[1].getNorm()).inv())));
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
}
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
}
std::map<int, Transform> optimizedPoses;
@@ -1450,7 +1470,7 @@ Transform RegistrationVis::computeTransformationImpl(
info.inliers = inliersCount;
info.matches = matchesCount;
info.rejectedMsg = msg;
info.covariance = covariance.at<double>(0,0)>0.0001?covariance:cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
info.covariance = covariance;
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;

2
corelib/src/RtabmapThread.cpp
View File

@@ -589,7 +589,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
double maxTransVar = odomEvent.transVariance();
if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
{
covariance_ = (covariance_.inv() + odomEvent.covariance().inv()).inv();
covariance_ += odomEvent.covariance();
}
else
{