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

holonomic option: makeing z depending on x and pitch too if nonholonomic

Browse Source
This commit is contained in:
matlabbe
2015-06-16 08:50:32 -04:00
parent bc18d4bf7d
commit 7cd0d0cd53
3 changed files with 14 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -322,7 +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, Holonomic, bool, true, "If the robot is holonomic (strafing and up/down commands can be issued). If not, y/z values will be estimated from x and rotation values (y=x*tan(yaw), z=x*tan(pitch)).");
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.");

13
corelib/src/Odometry.cpp
View File

@@ -225,9 +225,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
if(!_force2D)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
if(_holonomic)
{
z = filters_[2]->filter(z);
}
else
{
z = x * tan(pitch);
}
}
}
@@ -239,6 +246,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
else if(!_holonomic)
{
y = x * tan(yaw);
if(!_force2D)
{
z = x * tan(pitch);
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),