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

Parameters: added RGBD/LoopCovLimited, refactored "detect more loop closures" in MainWindow/DBViewer/rtabmap

Browse Source
This commit is contained in:
matlabbe
2018-10-01 20:22:20 -04:00
parent 0059a4bc1b
commit b5dec56eaf
14 changed files with 468 additions and 319 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/include/rtabmap/core/Graph.h
View File

@@ -102,6 +102,18 @@ Transform RTABMAP_EXP calcRMSE(
float & rotational_min,
float & rotational_max);
void RTABMAP_EXP computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink = 0,
const Link ** maxAngularErrorLink = 0);
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links);
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -206,6 +206,7 @@ public:
int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
const Feature2D * getFeature2D() const {return _feature2D;}
bool isGraphReduced() const {return _reduceGraph;}
const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
@@ -315,6 +316,7 @@ private:
GPS _gpsOrigin;
std::vector<CameraModel> _rectCameraModels;
StereoCameraModel _rectStereoCameraModel;
std::vector<double> _odomMaxInf;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -353,6 +353,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes.");
RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure, bool, false, "Do local bundle adjustment with neighborhood of the loop closure.");
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");

2
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -168,6 +168,7 @@ public:
std::map<int, Signature> * signatures = 0);
int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1, const ProgressState * state = 0);
int refineLinks();
cv::Mat getInformation(const cv::Mat & covariance) const;
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
@@ -259,6 +260,7 @@ private:
float _pathLinearVelocity;
float _pathAngularVelocity;
bool _savedLocalizationIgnored;
bool _loopCovLimited;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;

86
corelib/src/Graph.cpp
View File

@@ -818,6 +818,92 @@ Transform calcRMSE (
return t;
}
void computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink)
{
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
UASSERT(iter->second.transVariance()>0.0);
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
if(maxLinearErrorLink)
{
*maxLinearErrorLink = &iter->second;
}
}
UASSERT(iter->second.rotVariance()>0.0);
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
{
*maxAngularErrorLink = &iter->second;
}
}
}
}
}
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
{
std::vector<double> maxOdomInf(6,0.0);
maxOdomInf.resize(6,0.0);
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
{
for(int i=0; i<6; ++i)
{
const double & v = iter->second.infMatrix().at<double>(i,i);
if(maxOdomInf[i] < v)
{
maxOdomInf[i] = v;
}
}
}
}
if(maxOdomInf[0] == 0.0)
{
maxOdomInf.clear();
}
return maxOdomInf;
}
////////////////////////////////////////////
// Graph utilities

39
corelib/src/Memory.cpp
View File

@@ -239,6 +239,29 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
// global loop closures.
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
_workingMem.insert(std::make_pair((*iter)->id(), UTimer::now()));
//update odomMaxInf vector
std::multimap<int, Link> links = this->getAllLinks(true, true);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor &&
iter->second.infMatrix().cols == 6 &&
iter->second.infMatrix().rows == 6)
{
if(_odomMaxInf.empty())
{
_odomMaxInf.resize(6, 0.0);
}
for(int i=0; i<6; ++i)
{
const double & v = iter->second.infMatrix().at<double>(i,i);
if(_odomMaxInf[i] < v)
{
_odomMaxInf[i] = v;
}
}
}
}
}
else
{
@@ -832,6 +855,21 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
std::cout << "Covariance: " << covariance << std::endl;
infMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
UASSERT(infMatrix.rows == 6 && infMatrix.cols == 6);
if(_odomMaxInf.empty())
{
_odomMaxInf.resize(6, 0.0);
}
for(int i=0; i<6; ++i)
{
const double & v = infMatrix.at<double>(i,i);
if(_odomMaxInf[i] < v)
{
_odomMaxInf[i] = v;
}
}
motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
@@ -1519,6 +1557,7 @@ void Memory::clear()
_gpsOrigin = GPS();
_rectCameraModels.clear();
_rectStereoCameraModel = StereoCameraModel();
_odomMaxInf.clear();
if(_dbDriver)
{

192
corelib/src/Rtabmap.cpp
View File

@@ -123,6 +123,7 @@ Rtabmap::Rtabmap() :
_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -464,6 +465,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1348,7 +1350,7 @@ bool Rtabmap::process(
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.covariance.inv())))
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
{
++proximityDetectionsInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1916,11 +1918,6 @@ bool Rtabmap::process(
transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
{
loopClosureLinearVariance = info.covariance.at<double>(0,0);
loopClosureAngularVariance = info.covariance.at<double>(3,3);
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
@@ -1936,7 +1933,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.covariance.inv()));
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -2043,7 +2043,8 @@ bool Rtabmap::process(
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
cv::Mat information = getInformation(info.covariance);
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, information));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
@@ -2053,11 +2054,9 @@ bool Rtabmap::process(
loopClosureVisualInliers = info.inliers;
loopClosureVisualMatches = info.matches;
if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
{
loopClosureLinearVariance = info.covariance.at<double>(0,0);
loopClosureAngularVariance = info.covariance.at<double>(3,3);
}
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
}
}
else
@@ -2184,7 +2183,7 @@ bool Rtabmap::process(
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, (info.covariance*100.0).inv(), scanMatchingIds));
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++proximityDetectionsAddedByICPOnly;
@@ -2348,46 +2347,23 @@ bool Rtabmap::process(
optimizationIterations > 0 &&
constraints.size())
{
UINFO("Compute max graph errors...");
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
graph::computeMaxGraphErrors(
poses,
constraints,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink == 0 && maxAngularLink==0)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
float opt_roll,opt__pitch,opt__yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
fabs(opt_roll - link_roll),
fabs(opt__pitch - link_pitch),
fabs(opt__yaw - link_yaw));
float stddevLinear = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
maxLinearLink = &iter->second;
}
float stddevAngular = sqrt(iter->second.rotVariance());
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
maxAngularLink = &iter->second;
}
}
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
}
bool reject = false;
if(maxLinearLink)
{
@@ -3411,7 +3387,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
UERROR("Failed to optimize the graph! returning empty optimized poses...");
UWARN("Failed to optimize the graph! returning empty optimized poses...");
optimizedPoses.clear();
if(constraints)
{
@@ -3941,11 +3917,13 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
}
}
std::multimap<int, Link> linksIn = links;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.covariance.inv())));
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
std::multimap<int, Link> links;
UASSERT(poses.find(fromId) != poses.end());
@@ -3960,60 +3938,52 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
std::string msg;
if(optimizedPoses.size())
{
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
// ignore links with high variance
if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
{
UASSERT(optimizedPoses.find(iter->second.from())!=optimizedPoses.end());
UASSERT(optimizedPoses.find(iter->second.to())!=optimizedPoses.end());
Transform t1 = optimizedPoses.at(iter->second.from());
Transform t2 = optimizedPoses.at(iter->second.to());
UASSERT(!t1.isNull() && !t2.isNull());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
Eigen::Vector3f vA = t1.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = t2.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(linearError > maxLinearError)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
}
if(angularError > maxAngularError)
{
maxAngularError = angularError;
maxAngularLink = &iter->second;
}
}
}
graph::computeMaxGraphErrors(
optimizedPoses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > _optimizationMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
}
if(maxAngularLink)
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
}
if(maxLinearError > _optimizationMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
"\"%s\" is %f m.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxAngularError*180.0f/M_PI,
maxAngularLink?maxAngularLink->from():0,
maxAngularLink?maxAngularLink->to():0,
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
if(maxAngularErrorRatio > _optimizationMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
"\"%s\" is %f m.",
from,
to,
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
}
}
else
@@ -4034,7 +4004,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
cv::Mat inf = info.covariance.inv();
cv::Mat inf = getInformation(info.covariance);
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
@@ -4136,6 +4106,26 @@ int Rtabmap::refineLinks()
return (int)linksRefined.size();
}
cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
{
cv::Mat information = covariance.inv();
if(_loopCovLimited)
{
const std::vector<double> & odomMaxInf = _memory->getOdomMaxInf();
if(odomMaxInf.size() == 6)
{
for(int i=0; i<6; ++i)
{
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
}
return information;
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);

2
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -344,7 +344,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UERROR("GTSAM exception caught: %s", e.what());
UWARN("GTSAM exception caught: %s", e.what());
delete optimizer;
return optimizedPoses;
}