From 85edc57ba59a83de7721dea0c18120517c22a178 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 12 Jun 2019 16:03:52 -0400 Subject: [PATCH] Added RGBD/MaxLocalizationDistance parameter --- corelib/include/rtabmap/core/Parameters.h | 1 + corelib/include/rtabmap/core/Rtabmap.h | 1 + corelib/src/Rtabmap.cpp | 25 ++++-- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 93 ++++++++++++++--------- 5 files changed, 80 insertions(+), 41 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index fdab8056..a7351de4 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -342,6 +342,7 @@ class RTABMAP_EXP Parameters 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 node 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, 3.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str())); + RTABMAP_PARAM(RGBD, MaxLocalizationDistance, float, 0.0, "Reject localizations if the distance from the map is over this distance (0=disabled). Only used in localization mode."); RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously."); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index c3486d4d..2c3d5259 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -230,6 +230,7 @@ private: unsigned int _maxMemoryAllowed; // signatures count in WM float _loopThr; float _loopRatio; + float _localizationMaxDistance; bool _verifyLoopClosureHypothesis; unsigned int _maxRetrieved; unsigned int _maxLocalRetrieved; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index b096ef85..edcf5fdf 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -87,6 +87,7 @@ Rtabmap::Rtabmap() : _maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf _loopThr(Parameters::defaultRtabmapLoopThr()), _loopRatio(Parameters::defaultRtabmapLoopRatio()), + _localizationMaxDistance(Parameters::defaultRGBDMaxLocalizationDistance()), _verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()), _maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()), _maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()), @@ -441,6 +442,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed); Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr); Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio); + Parameters::parse(parameters, Parameters::kRGBDMaxLocalizationDistance(), _localizationMaxDistance); Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis); Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved); Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved); @@ -2143,6 +2145,12 @@ bool Rtabmap::process( UWARN("Rejected loop closure %d -> %d: %s", _loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str()); } + else if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && transform.getNorm() > _localizationMaxDistance) + { + rejectedHypothesis = true; + UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.", + _loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLocalizationDistance().c_str(), _localizationMaxDistance); + } else { transform = transform.inverse(); @@ -2266,6 +2274,11 @@ bool Rtabmap::process( } UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths); + float proximityFilteringRadius = _proximityFilteringRadius; + if(!_memory->isIncremental() && _localizationMaxDistance>0.0f && (proximityFilteringRadius <= 0.0f || _localizationMaxDistance >::const_reverse_iterator iter=nearestPaths.rbegin(); iter!=nearestPaths.rend() && (_memory->isIncremental() || lastProximitySpaceClosureId == 0) && @@ -2294,8 +2307,8 @@ bool Rtabmap::process( { // nearest pose must not be linked to current location and enough close if(!signature->hasLink(nearestId) && - (_proximityFilteringRadius <= 0.0f || - _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius)) + (proximityFilteringRadius <= 0.0f || + _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < proximityFilteringRadius*proximityFilteringRadius)) { ++localVisualPathsChecked; RegistrationInfo info; @@ -2304,7 +2317,7 @@ bool Rtabmap::process( if(!transform.isNull()) { transform = transform.inverse(); - if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius) + if(proximityFilteringRadius <= 0 || transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius) { UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s", signature->id(), @@ -2331,7 +2344,7 @@ bool Rtabmap::process( { UWARN("Ignoring local loop closure with %d because resulting " "transform is too large!? (%fm > %fm)", - nearestId, transform.getNorm(), _proximityFilteringRadius); + nearestId, transform.getNorm(), proximityFilteringRadius); } } } @@ -2414,10 +2427,10 @@ bool Rtabmap::process( } std::map filteredPath; - if(optimizedLocalPath.size() > 2 && _proximityFilteringRadius > 0.0f) + if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f) { // path filtering - filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, _proximityFilteringRadius, 0, true); + filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true); // make sure the current pose is still here filteredPath.insert(*optimizedLocalPath.find(nearestId)); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 03fc1c08..803d7c6f 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -947,6 +947,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDProximityByTime().c_str()); _ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDProximityBySpace().c_str()); _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str()); + _ui->maxLocalizationDistance->setObjectName(Parameters::kRGBDMaxLocalizationDistance().c_str()); _ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDProximityMaxGraphDepth().c_str()); _ui->localDetection_maxNeighbors->setObjectName(Parameters::kRGBDProximityPathMaxNeighbors().c_str()); _ui->localDetection_maxPaths->setObjectName(Parameters::kRGBDProximityMaxPaths().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index ab8244dc..c2a06db4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 - 680 - 3082 + -354 + 681 + 3071 @@ -95,7 +95,7 @@ QFrame::Raised - 5 + 12 @@ -9629,6 +9629,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + @@ -9655,8 +9662,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -9672,13 +9679,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - @@ -9731,6 +9731,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + @@ -9757,13 +9764,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - @@ -9790,6 +9790,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + @@ -9819,13 +9826,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - @@ -9852,6 +9852,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 9999 + + + @@ -9881,14 +9888,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 9999 + + + + - + Maximum odometry cache size. Used only in localization mode and when maximum graph error parameter is not null. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching. @@ -9914,10 +9921,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Reject localizations if the distance from the map is over this distance (0=disabled). Only used in localization mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 1.000000000000000