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

Odometry: Added process() with optional guess interface

Browse Source
This commit is contained in:
matlabbe
2016-06-30 11:34:46 -04:00
parent 4115bc416e
commit c2a7b2f13a
2 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Odometry.cpp
View File

@@ -194,6 +194,11 @@ void Odometry::reset(const Transform & initialPose)
}
Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
return process(data, Transform(), info);
}
Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
{
UASSERT(!data.imageRaw().empty());
@@ -261,7 +266,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_?Transform::getIdentity():Transform();
Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());
if(!previousVelocityTransform_.isNull())
{
@@ -287,6 +292,11 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
}
}
if(!guessIn.isNull())
{
guess = guessIn;
}
UTimer time;
Transform t;
if(_imageDecimation > 1)