i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added odometry honolomic parameter

Browse Source
This commit is contained in:
matlabbe
2015-06-15 20:53:31 -04:00
parent dfbf6e721e
commit 53f7719655
6 changed files with 52 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Odometry.h
View File

@@ -77,6 +77,7 @@ private:
float _maxDepth;
int _resetCountdown;
bool _force2D;
bool _holonomic;
bool _particleFiltering;
int _particleSize;
float _particleNoiseT;

1
corelib/include/rtabmap/core/Parameters.h
View File

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

19
corelib/src/Odometry.cpp
View File

@@ -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;
}

1
guilib/src/PreferencesDialog.cpp
View File

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

2
guilib/src/ui/mainWindow.ui
View File

@@ -55,6 +55,7 @@
<property name="title">
<string>Advanced</string>
</property>
<addaction name="actionOpen_working_directory"/>
<addaction name="actionDump_the_memory"/>
<addaction name="actionDump_the_prediction_matrix"/>
<addaction name="actionGenerate_map"/>
@@ -73,7 +74,6 @@
<addaction name="separator"/>
<addaction name="actionDelete_memory"/>
<addaction name="separator"/>
<addaction name="actionOpen_working_directory"/>
<addaction name="menuAdvanced"/>
</widget>
<widget class="QMenu" name="menu6">

47
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-450</y>
<y>0</y>
<width>755</width>
<height>1591</height>
</rect>
@@ -6794,13 +6794,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="odom_countdown">
<property name="maximum">
<number>999999</number>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_44">
<property name="text">
@@ -6838,7 +6831,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_221">
<property name="text">
<string>Fill info with data (inliers/outliers features to be shown in Odometry view).</string>
@@ -6848,7 +6841,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QPushButton" name="pushButton_testOdometry">
<property name="text">
<string>Test selected odometry</string>
@@ -6875,7 +6868,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_232">
<property name="text">
<string>Data buffer size (0 means inf).</string>
@@ -6885,7 +6878,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QSpinBox" name="odom_dataBufferSize">
<property name="maximum">
<number>999999</number>
@@ -6902,7 +6895,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QCheckBox" name="odom_fillInfoData">
<property name="text">
<string/>
@@ -6956,7 +6949,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_233">
<property name="text">
<string>Particle filtering to smooth the odometry trajectory. See &quot;Particle Filter&quot; panel for the related parameters.</string>
@@ -6966,13 +6959,37 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QCheckBox" name="odom_particleFiltering">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="odom_countdown">
<property name="maximum">
<number>999999</number>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_238">
<property name="text">
<string>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)).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="odom_holonomic">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>