i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed changing local transform not handled correctly when changing for third party odometries

Browse Source
This commit is contained in:
matlabbe
2017-07-07 19:41:11 -04:00
parent f9ed36ed54
commit 9e011d6ad9
9 changed files with 51 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OdometryFovis.cpp
+12 -1
View File
@@ -111,6 +111,7 @@ void OdometryFovis::reset(const Transform & initialPose)
stereoDepth_ = 0;
}
lost_ = false;
previousLocalTransform_.setNull();
#endif
}
@@ -341,12 +342,14 @@ Transform OdometryFovis::computeTransform(
t.setNull();
lost_ = true;
covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
previousLocalTransform_.setNull();
}
else if(lost_)
{
lost_ = false;
// we are not lost anymore but we don't know where we are now according to last valid pose
covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
previousLocalTransform_.setNull();
}
else
{
@@ -362,7 +365,15 @@ Transform OdometryFovis::computeTransform(
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
t = localTransform * t * localTransform.inverse();
if(!previousLocalTransform_.isNull())
{
t = previousLocalTransform_ * t * localTransform.inverse();
}
else
{
t = localTransform * t * localTransform.inverse();
}
previousLocalTransform_ = localTransform;
}
if(info)