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

Added parameter "RGBD/OptimizeMaxError" (default: 1 m) to reject loop closures after too large optimization errors

Browse Source
This commit is contained in:
matlabbe
2015-09-03 12:37:14 -04:00
parent 786bbaa2c2
commit 198899ba32
6 changed files with 194 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+12 -11
View File
@@ -284,18 +284,19 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -191,6 +191,7 @@ private:
bool _localPathOdomPosesUsed;
std::string _databasePath;
bool _optimizeFromGraphEnd;
float _optimizationMaxLinearError;
bool _reextractLoopClosureFeatures;
int _reextractNNType;
float _reextractNNDR;
@@ -205,6 +206,7 @@ private:
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
double _lastProcessTime;
bool _someNodesHaveBeenTransferred;
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.
corelib/include/rtabmap/core/Statistics.h
+3 -2
View File
@@ -57,11 +57,12 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
RTABMAP_STATS(Loop, ReactivateId,);
RTABMAP_STATS(Loop, Reactivate_id,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
RTABMAP_STATS(Loop, VisualInliers,);
RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(Loop, Optimization_max_error, m);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
corelib/src/Rtabmap.cpp
+139 -42
View File
@@ -100,6 +100,7 @@ Rtabmap::Rtabmap() :
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
_reextractNNType(Parameters::defaultLccReextractNNType()),
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
@@ -113,6 +114,7 @@ Rtabmap::Rtabmap() :
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
_someNodesHaveBeenTransferred(false),
_epipolarGeometry(0),
_bayesFilter(0),
_graphOptimizer(0),
@@ -319,6 +321,7 @@ void Rtabmap::close()
_highestHypothesis = std::make_pair(0,0.0f);
_loopClosureHypothesis = std::make_pair(0,0.0f);
_lastProcessTime = 0.0;
_someNodesHaveBeenTransferred = false;
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
@@ -398,6 +401,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
@@ -829,6 +833,7 @@ void Rtabmap::resetMemory()
_highestHypothesis = std::make_pair(0,0.0f);
_loopClosureHypothesis = std::make_pair(0,0.0f);
_lastProcessTime = 0.0;
_someNodesHaveBeenTransferred = false;
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
@@ -902,7 +907,6 @@ bool Rtabmap::process(
std::map<int, int> childCount;
std::set<int> signaturesRetrieved;
int localLoopClosuresInTimeFound = 0;
bool scanMatchingSuccess = false;
const Signature * signature = 0;
const Signature * sLoop = 0;
@@ -1044,49 +1048,77 @@ bool Rtabmap::process(
}
}
Transform newPose = _mapCorrection * signature->getPose();
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
//============================================================
// Scan matching
//============================================================
if(_poseScanMatching &&
signature->getLinks().size() == 1 &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
// Update optimizedPoses with the newly added node
Transform newPose;
if(signature->getLinks().size() == 1)
{
UINFO("Odometry correction by scan matching");
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
std::string rejectedMsg;
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
int inliers = 0;
float inliersRatio = 0;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
//============================================================
// Scan matching
//============================================================
if(_poseScanMatching &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
scanMatchingSuccess = true;
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance, variance);
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
signature->id(),
oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance, variance);
if(_optimizeFromGraphEnd)
{
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t;
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * iter->second * u;
}
}
}
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
}
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
}
else
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
_mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
else
{
newPose = _mapCorrection * signature->getPose();
}
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
if(signature->getLinks().size() == 1)
{
@@ -1543,7 +1575,7 @@ bool Rtabmap::process(
if(immunizedLocally >= maxLocalLocationsImmunized)
{
// set 20 to avoid this warning when starting mapping
if(maxLocalLocationsImmunized > 20)
if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred)
{
UWARN("Could not immunize the whole local path (%d) between "
"%d and %d (max location immunized=%d). You may want "
@@ -1675,6 +1707,7 @@ bool Rtabmap::process(
// Update loop closure links
// (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM)
//=============================================================
std::list<std::pair<int, int> > loopClosureLinksAdded;
int loopClosureVisualInliers = 0; // for statistics
if(_loopClosureHypothesis.first>0)
{
@@ -1758,6 +1791,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
if(!rejectedHypothesis)
{
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
}
}
if(rejectedHypothesis)
@@ -1911,6 +1948,7 @@ bool Rtabmap::process(
nearestId,
transform.prettyPrint().c_str());
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
{
@@ -2000,6 +2038,7 @@ bool Rtabmap::process(
transform.prettyPrint().c_str());
// set Identify covariance for laser scan matching only
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
++localSpaceClosuresAddedByICPOnly;
@@ -2027,10 +2066,10 @@ bool Rtabmap::process(
//============================================================
// Optimize map graph
//============================================================
float maxLinearError = 0.0f;
if(_rgbdSlamMode &&
(_loopClosureHypothesis.first>0 || // can be different map of the current one
(_loopClosureHypothesis.first>0 || // can be different map of the current one
localLoopClosuresInTimeFound>0 || // only same map of the current one
scanMatchingSuccess || // only same map of the current one
lastLocalSpaceClosureId>0 || // can be different map of the current one
signaturesRetrieved.size())) // can be different map of the current one
{
@@ -2038,8 +2077,61 @@ bool Rtabmap::process(
{
UINFO("Update map correction: SLAM mode");
// SLAM mode!
optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> constraints;
optimizeCurrentMap(signature->id(), false, poses, &constraints);
UASSERT(poses.find(signature->id()) != poses.end());
// Check added loop closures have broken the graph
// (in case of wrong loop closures).
bool updateConstraints = true;
if(_optimizationMaxLinearError > 0.0f && loopClosureLinksAdded.size())
{
const Link * maxLinearLink = 0;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
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()));
if(linearError > maxLinearError)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
}
}
if(maxLinearError > _optimizationMaxLinearError)
{
UWARN("Rejecting all added loop closures (%d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error of %f m (edge %d->%d). The "
"maximum error parameter is %f m.",
(int)loopClosureLinksAdded.size(),
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastLocalSpaceClosureId = 0;
rejectedHypothesis = true;
}
}
if(updateConstraints)
{
_optimizedPoses = poses;
_constraints = constraints;
}
// Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
@@ -2138,10 +2230,11 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis);
statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
@@ -2273,6 +2366,10 @@ bool Rtabmap::process(
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
std::list<int> transferred = _memory->forget(immunizedLocations);
signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
if(!_someNodesHaveBeenTransferred && transferred.size())
{
_someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization
}
}
_lastProcessTime = totalTime;
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -578,6 +578,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
guilib/src/ui/preferencesDialog.ui
+37 -2
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-392</y>
<y>-509</y>
<width>760</width>
<height>1570</height>
<height>1598</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -6735,6 +6735,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_259">
<property name="text">
<string>Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="graphOptimization_maxError">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>