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

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

14
corelib/src/OdometryViso2.cpp
View File

@@ -86,6 +86,7 @@ void OdometryViso2::reset(const Transform & initialPose)
}
lost_ = false;
reference_motion_.setIdentity();
previousLocalTransform_.setNull();
#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
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)