i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Odometry: added force2D option

Browse Source 
Changed parameter RGBD/ScanMatchingSize to RGBD/PoseScanMatching

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@2050 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-19 23:38:15 +00:00
parent 89bcea3057
commit faf43cc82c
9 changed files with 127 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+28 -2
View File
@@ -68,6 +68,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
@@ -82,12 +83,27 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
}
void Odometry::reset(const Transform & initialPose)
{
_resetCurrentCount = 0;
_pose = initialPose;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
{
UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
}
Transform pose(x, y, 0, 0, 0, yaw);
_pose = pose;
}
else
{
_pose = initialPose;
}
}
bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -110,7 +126,17 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
{
_resetCurrentCount = _resetCountdown;
_pose *= t;
if(_force2D)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
_pose *= Transform(x,y,0,0,0,yaw);
}
else
{
_pose *= t;
}
return _pose;
}
else if(_resetCurrentCount > 0)