i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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;
}