i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdometryViso2: updated for dynamic local transform

Browse Source
This commit is contained in:
matlabbe
2017-07-10 17:34:37 -04:00
parent 9e011d6ad9
commit dc77bb4332
2 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/OdometryViso2.h
View File

@@ -54,6 +54,7 @@ private:
bool lost_; bool lost_;
bool keep_reference_frame_; bool keep_reference_frame_;
Transform reference_motion_; Transform reference_motion_;
Transform previousLocalTransform_;
ParametersMap viso2Parameters_; ParametersMap viso2Parameters_;
}; };

14
corelib/src/OdometryViso2.cpp
View File

@@ -86,6 +86,7 @@ void OdometryViso2::reset(const Transform & initialPose)
} }
lost_ = false; lost_ = false;
reference_motion_.setIdentity(); reference_motion_.setIdentity();
previousLocalTransform_.setNull();
#endif #endif
} }
@@ -253,10 +254,19 @@ Transform OdometryViso2::computeTransform(
} }
} }
if(!t.isNull() && !t.isIdentity() && !data.stereoCameraModel().localTransform().isIdentity() && !data.stereoCameraModel().localTransform().isNull()) const Transform & localTransform = data.stereoCameraModel().localTransform();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{ {
// from camera frame to base frame // from camera frame to base frame
t = data.stereoCameraModel().localTransform() * t * data.stereoCameraModel().localTransform().inverse(); if(!previousLocalTransform_.isNull())
{
t = previousLocalTransform_ * t * localTransform.inverse();
}
else
{
t = localTransform * t * localTransform.inverse();
}
previousLocalTransform_ = localTransform;
} }
if(info) if(info)