fixed introlab/rtabmap_ros#1134

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.");

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;

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())
  {

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());