diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index dedb85bf..4a805652 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -77,6 +77,7 @@ private: float _maxDepth; int _resetCountdown; bool _force2D; + bool _holonomic; bool _particleFiltering; int _particleSize; float _particleNoiseT; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index bce3b2fe..dc3bb3f7 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -322,6 +322,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); + RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); RTABMAP_PARAM(Odom, PnPEstimation, bool, false, "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences."); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ce5f5c08..6608dead 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -43,6 +43,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _maxDepth(Parameters::defaultOdomMaxDepth()), _resetCountdown(Parameters::defaultOdomResetCountdown()), _force2D(Parameters::defaultOdomForce2D()), + _holonomic(Parameters::defaultOdomHolonomic()), _particleFiltering(Parameters::defaultOdomParticleFiltering()), _particleSize(Parameters::defaultOdomParticleSize()), _particleNoiseT(Parameters::defaultOdomParticleNoiseT()), @@ -66,6 +67,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth); Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios); Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D); + Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic); Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData); Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation); Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError); @@ -191,7 +193,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) { _resetCurrentCount = _resetCountdown; - if(_force2D || filters_.size()) + if(_force2D || !_holonomic || filters_.size()) { float x,y,z, roll,pitch,yaw; t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); @@ -211,8 +213,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) else { x = filters_[0]->filter(x); - y = filters_[1]->filter(y); yaw = filters_[5]->filter(yaw); + if(_holonomic) + { + y = filters_[1]->filter(y); + } + else + { + y = x * tan(yaw); + } if(!_force2D) { @@ -227,13 +236,17 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) info->timeParticleFiltering = time.ticks(); } } + else if(!_holonomic) + { + y = x * tan(yaw); + } UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw), uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s", x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str()); t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw); - if(info) + if(info && filters_.size()) { info->transformFiltered = t; } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index bc9d9650..09943ff3 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -550,6 +550,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str()); _ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str()); _ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str()); + _ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str()); _ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str()); _ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str()); _ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str()); diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 91aa6443..3b84d7a0 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -55,6 +55,7 @@ Advanced + @@ -73,7 +74,6 @@ - diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 160414e7..0acaa790 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -450 + 0 755 1591 @@ -6794,13 +6794,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - 999999 - - - @@ -6838,7 +6831,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Fill info with data (inliers/outliers features to be shown in Odometry view). @@ -6848,7 +6841,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Test selected odometry @@ -6875,7 +6868,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Data buffer size (0 means inf). @@ -6885,7 +6878,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + 999999 @@ -6902,7 +6895,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + @@ -6956,7 +6949,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + Particle filtering to smooth the odometry trajectory. See "Particle Filter" panel for the related parameters. @@ -6966,13 +6959,37 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + + + + 999999 + + + + + + + If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). + + + true + + + + + + + + + +