diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 790ab0aebb..5f1bcf43fa 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -358,6 +358,7 @@ class RTABMAP_CORE_EXPORT Parameters 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, MaxLoopClosureDistance, float, 0.0, "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled)."); + RTABMAP_PARAM(RGBD, ForceOdom3DoF, bool, true, uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str())); RTABMAP_PARAM(RGBD, StartAtOrigin, bool, false, uFormat("If true, rtabmap will assume the robot is starting 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 (%s=false).", kMemIncrementalMemory().c_str())); 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 48676f5c6a..d4b69c1fde 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -322,6 +322,7 @@ class RTABMAP_CORE_EXPORT Rtabmap int _pathStuckIterations; float _pathLinearVelocity; float _pathAngularVelocity; + bool _forceOdom3doF; bool _restartAtOrigin; bool _loopCovLimited; bool _loopGPS; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index fc2c8cd57c..7a636e6e08 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -143,6 +143,7 @@ Rtabmap::Rtabmap() : _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()), _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()), _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()), + _forceOdom3doF(Parameters::defaultRGBDForceOdom3DoF()), _restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()), _loopCovLimited(Parameters::defaultRGBDLoopCovLimited()), _loopGPS(Parameters::defaultRtabmapLoopGPS()), @@ -616,6 +617,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations); Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity); Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity); + Parameters::parse(parameters, Parameters::kRGBDForceOdom3DoF(), _forceOdom3doF); Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin); Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited); Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS); @@ -1252,6 +1254,12 @@ bool Rtabmap::process( { if(!odomPose.isNull()) { + // If we are doing 2D mapping, make sure the pose is 3DoF so that landmark logic works. + if(_forceOdom3doF && _graphOptimizer->isSlam2d() && !odomPose.is3DoF()) + { + odomPose = odomPose.to3DoF(); + } + // this will make sure that all inverse operations will work! if(!odomPose.isInvertible()) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 42c1087952..740c2fa335 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1114,6 +1114,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str()); _ui->rgdb_linearSpeedUpdate->setObjectName(Parameters::kRGBDLinearSpeedUpdate().c_str()); _ui->rgdb_angularSpeedUpdate->setObjectName(Parameters::kRGBDAngularSpeedUpdate().c_str()); + _ui->rgbd_forceOdom3DoF->setObjectName(Parameters::kRGBDForceOdom3DoF().c_str()); _ui->rgbd_savedLocalizationIgnored->setObjectName(Parameters::kRGBDStartAtOrigin().c_str()); _ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str()); _ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 43a5882916..5a290087bd 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -836 + 0 713 4002 @@ -95,7 +95,7 @@ QFrame::Raised - 5 + 12 @@ -11541,10 +11541,34 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Map Update - - + + - Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. + + + + + + + + + + + + + + + m + + + 1.000000000000000 + + + + + + + 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 @@ -11554,10 +11578,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. + Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. true @@ -11567,23 +11591,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + m - 3 + 1 + + + 99.000000000000000 0.100000000000000 + + 1.000000000000000 + - - + + - Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. + Maximum angular speed to update the map (0 means not limit). true @@ -11593,10 +11623,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 @@ -11606,34 +11636,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - - - - - - - - m + 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. - - 1.000000000000000 + + true - - - - - - 9999 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse @@ -11653,10 +11665,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. + Re-extract visual features when computing loop closure transformations. Raw features are not saved in database. true @@ -11666,7 +11678,20 @@ 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + 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. @@ -11679,43 +11704,63 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - - + + + + Do local bundle adjustment with neighborhood of the loop closure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + m + + 1.000000000000000 + + + + + + + rad/s + - 1 + 2 - 99.000000000000000 + 3.140000000000000 0.100000000000000 - - 1.000000000000000 - - - + + - - + + - 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. + Localization smoothing. Used only in localization mode. Adjust localization constraints based on optimized odometry cache poses. true @@ -11725,20 +11770,17 @@ 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. + 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 @@ -11748,9 +11790,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - @@ -11764,10 +11803,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Maximum linear 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 @@ -11777,14 +11823,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - + Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess). @@ -11797,59 +11836,62 @@ 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 + + 2 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 1.000000000000000 + + + 0.100000000000000 + + + 0.100000000000000 - - + + - Inverted registration. On loop closure, do registration from the target to reference instead of reference to target. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - rad/s + m - 2 + 4 - 3.140000000000000 + 1.000000000000000 - 0.100000000000000 + 0.010000000000000 + + + 0.010000000000000 - - + + - 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 linear speed to update the map (0 means not limit). true @@ -11859,17 +11901,26 @@ 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Maximum angular speed to update the map (0 means not limit). + 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 @@ -11879,29 +11930,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - + + + - - 2 - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.100000000000000 - - - + + - 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. + Linear update: Minimum linear displacement to update the map. Note that Weight Update is done prior to this, so weights are still updated. true @@ -11911,10 +11950,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + m + + + 3 + + + 0.100000000000000 + + + + + - Ignore off diagonal values of the odometry covariance matrix. + 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 @@ -11924,10 +11976,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - 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). + Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels. true @@ -11938,9 +11990,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - 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. + Reject loop closures/localizations if the distance from the map is over this distance (0=disabled). true @@ -11950,24 +12002,24 @@ 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 @@ -11977,17 +12029,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - + - 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. + Force odometry poses to be 3DoF if Motion Estimation is 3DoF. true @@ -11997,35 +12042,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - 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 - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 4 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 +