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

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

@@ -55,12 +55,14 @@ public:
public: public:
virtual ~Odometry(); virtual ~Odometry();
Transform process(SensorData & data, OdometryInfo * info = 0); Transform process(SensorData & data, OdometryInfo * info = 0);
Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
//getters //getters
const Transform & getPose() const {return _pose;} const Transform & getPose() const {return _pose;}
bool isInfoDataFilled() const {return _fillInfoData;} bool isInfoDataFilled() const {return _fillInfoData;}
const Transform & previousVelocityTransform() const {return previousVelocityTransform_;} const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
double previousStamp() const {return previousStamp_;}
private: private:
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0; virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;

12
corelib/src/Odometry.cpp
View File

@@ -194,6 +194,11 @@ void Odometry::reset(const Transform & initialPose)
} }
Transform Odometry::process(SensorData & data, OdometryInfo * info) 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()); 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; 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()); 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()) if(!previousVelocityTransform_.isNull())
{ {
@@ -287,6 +292,11 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
} }
} }
if(!guessIn.isNull())
{
guess = guessIn;
}
UTimer time; UTimer time;
Transform t; Transform t;
if(_imageDecimation > 1) if(_imageDecimation > 1)