From ece4bd76d13ae7caa350c9c7b20d05faf37d142c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 3 Nov 2023 17:19:49 -0700 Subject: [PATCH 1/2] tuning localization priors (for https://github.com/introlab/rtabmap_ros/issues/1057) --- corelib/include/rtabmap/core/Parameters.h | 1 + corelib/include/rtabmap/core/Rtabmap.h | 1 + corelib/src/Rtabmap.cpp | 39 ++- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 386 ++++++++++++---------- 5 files changed, 246 insertions(+), 182 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 7aee191a1b..41bcd3c1da 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -377,6 +377,7 @@ class RTABMAP_CORE_EXPORT 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."); RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); + RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.01, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); // Local/Proximity loop closure detection RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 2a4b23ae88..48676f5c6a 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -327,6 +327,7 @@ class RTABMAP_CORE_EXPORT Rtabmap bool _loopGPS; int _maxOdomCacheSize; bool _localizationSmoothing; + double _localizationPriorInf; bool _createGlobalScanMap; float _markerPriorsLinearVariance; float _markerPriorsAngularVariance; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 17153607ad..4ac883ab66 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -148,6 +148,7 @@ Rtabmap::Rtabmap() : _loopGPS(Parameters::defaultRtabmapLoopGPS()), _maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()), _localizationSmoothing(Parameters::defaultRGBDLocalizationSmoothing()), + _localizationPriorInf(1.0/(Parameters::defaultRGBDLocalizationPriorError()*Parameters::defaultRGBDLocalizationPriorError())), _createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()), _markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()), _markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()), @@ -620,6 +621,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS); Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize); Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing); + double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError(); + Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError); + UASSERT(localizationPriorError>0.0); + _localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError); Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap); Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance); @@ -3137,15 +3142,19 @@ bool Rtabmap::process( { constraints.insert(std::make_pair(iter->second.from(), iter->second)); } + cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) { std::map::iterator iterPose = _optimizedPoses.find(iter->second.to()); if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end()) { poses.insert(*iterPose); - // make the poses in the map fixed - constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000))); - UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str()); + if(iterPose->first > 0) + { + // make the poses in the map fixed + constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); + UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str()); + } } UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance()); } @@ -3163,7 +3172,17 @@ bool Rtabmap::process( // If slam2d: get connected graph while keeping original roll,pitch,z values. _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); cv::Mat locOptCovariance; - std::map optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + std::map optPoses; + if(poses.lower_bound(1) != poses.end() && + _odomCachePoses.lower_bound(1) != poses.end() && + poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first) + { + optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { @@ -3303,7 +3322,17 @@ bool Rtabmap::process( _graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map // If slam2d: get connected graph while keeping original roll,pitch,z values. _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d()); - optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance); + optPoses.clear(); + if(poses.lower_bound(1) != poses.end() && + _odomCachePoses.lower_bound(1) != poses.end() && + poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first) + { + optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance); + } + else + { + UERROR("Invalid localization constraints"); + } _graphOptimizer->setPriorsIgnored(priorsIgnored); // set back for(std::map::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index ef2e7e5bdc..d65c0adac9 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1154,6 +1154,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str()); _ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str()); _ui->checkbox_localizationSmoothing->setObjectName(Parameters::kRGBDLocalizationSmoothing().c_str()); + _ui->doubleSpinBox_localizationPriorError->setObjectName(Parameters::kRGBDLocalizationPriorError().c_str()); // Registration _ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index b76f02f92c..f6d3dd1cf1 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -942 + -876 756 - 3736 + 4371 @@ -95,7 +95,7 @@ QFrame::Raised - 21 + 12 @@ -11295,29 +11295,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Map Update - - - - m + + + + Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. - - 1 + + true - - 99.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - 0.100000000000000 + + + + + + Ratio of working memory for which local nodes are immunized from transfer. - - 1.000000000000000 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - m/s + m 3 @@ -11327,17 +11334,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. + Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. true @@ -11347,10 +11347,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). true @@ -11360,45 +11360,37 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. - - - true + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + - - + + - rad/s - - - 2 - - - 3.140000000000000 + m - - 0.100000000000000 + + 1.000000000000000 - - - - + + + + 9999 - - - @@ -11415,10 +11407,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. + Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. true @@ -11428,10 +11420,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. + Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. true @@ -11441,29 +11433,43 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - + + + + + + + + + m + - 2 + 1 - 1.000000000000000 + 99.000000000000000 0.100000000000000 - 0.100000000000000 + 1.000000000000000 - - + + - Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching. + + + + + + + + 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. true @@ -11473,34 +11479,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + m + + + 1.000000000000000 - - - - 9999 + + + + Do local bundle adjustment with neighborhood of the loop closure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + + - m + m/s - - 1.000000000000000 + + 3 + + + 0.100000000000000 - - + + - 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). + Maximum linear speed to update the map (0 means not limit). true @@ -11510,6 +11531,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + @@ -11523,24 +11551,30 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - + + - + Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - 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. + Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. true @@ -11550,23 +11584,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - m + rad/s - 3 + 2 + + + 3.140000000000000 0.100000000000000 - - + + - Maximum linear speed to update the map (0 means not limit). + Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). true @@ -11576,24 +11613,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - - + + - Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required). + Maximum angular speed to update the map (0 means not limit). true @@ -11603,10 +11633,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + 2 + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 + + + + + - If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode. + Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if "Optimizer/GravitySigma" is not zero. true @@ -11616,10 +11665,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). + Ignore off diagonal values of the odometry covariance matrix. true @@ -11629,10 +11678,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Ratio of working memory for which local nodes are immunized from transfer. + 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). true @@ -11642,10 +11691,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching. true @@ -11655,40 +11704,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - - - m - - - 1.000000000000000 - - - - - + + - Ignore off diagonal values of the odometry covariance matrix. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - Do local bundle adjustment with neighborhood of the loop closure. + Angular update: Minimum angular displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11698,17 +11731,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - + + - Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. + If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode. true @@ -11718,10 +11751,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Maximum angular speed to update the map (0 means not limit). + Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. true @@ -11731,23 +11764,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. + + + + m - - true + + 3 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1.000000000000000 - - - - - - + + 0.010000000000000 + + + 0.010000000000000 @@ -17934,7 +17966,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag OpenVINS - + @@ -19046,7 +19078,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000000000001000 - 0.000000000000001 + 0.000000000000000 @@ -19259,7 +19291,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Accel "white noise" + Accel "white noise" true @@ -19323,7 +19355,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Gyro "white noise" + Gyro "white noise" true From 7c94caea0a6e4fd39b4b5297b9985b729f98be5b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 5 Nov 2023 13:47:02 -0800 Subject: [PATCH 2/2] Set prior error value to same than old default value --- corelib/include/rtabmap/core/Parameters.h | 2 +- guilib/src/ui/preferencesDialog.ui | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 41bcd3c1da..b93a2ba0b1 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -377,7 +377,7 @@ class RTABMAP_CORE_EXPORT 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."); RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str())); RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); - RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.01, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); + RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str())); // Local/Proximity loop closure detection RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM."); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index f6d3dd1cf1..ef9040d00d 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -876 + -844 756 4371 @@ -11770,7 +11770,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag m - 3 + 4 1.000000000000000