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

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

22
corelib/src/OdometryDVO.cpp
View File

@@ -90,6 +90,7 @@ void OdometryDVO::reset(const Transform & initialPose)
}
lost_ = false;
motionFromKeyFrame_.setIdentity();
previousLocalTransform_.setNull();
#endif
}
@@ -192,6 +193,7 @@ Transform OdometryDVO::computeTransform(
dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
const Transform & localTransform = data.cameraModels()[0].localTransform();
cv::Mat covariance;
if(reference_ == 0)
{
@@ -245,16 +247,24 @@ Transform OdometryDVO::computeTransform(
reference_ = 0; // this will make restart from the next frame
motionFromKeyFrame_.setIdentity();
t.setNull();
previousLocalTransform_.setNull();
covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
}
}
const Transform & localTransform = data.cameraModels()[0].localTransform();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
t = localTransform * t * localTransform.inverse();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
if(!previousLocalTransform_.isNull())
{
t = previousLocalTransform_ * t * localTransform.inverse();
}
else
{
t = localTransform * t * localTransform.inverse();
}
previousLocalTransform_ = localTransform;
}
}
if(info)

13
corelib/src/OdometryFovis.cpp
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)

17
corelib/src/OdometryORBSLAM2.cpp
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h"
#ifdef RTABMAP_ORB_SLAM2
#include <System.h>
@@ -495,6 +496,7 @@ public:
if(!vocabularyPath.empty())
{
//Load ORB Vocabulary
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
mpVocabulary = new ORB_SLAM2::ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
@@ -773,6 +775,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
orbslam2_->shutdown();
}
firstFrame_ = true;
originLocalTransform_.setNull();
#endif
}
@@ -845,14 +848,17 @@ Transform OdometryORBSLAM2::computeTransform(
}
else if(Tcw.cols == 4 && Tcw.rows == 4)
{
t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)).clone());
t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
t = localTransform * t.inverse() * localTransform.inverse();
if(originLocalTransform_.isNull())
{
originLocalTransform_ = localTransform;
}
t = originLocalTransform_ * t.inverse() * localTransform.inverse();
t = this->getPose().inverse() * t;
}
t = this->getPose().inverse() * t;
if(firstFrame_)
{
@@ -916,12 +922,13 @@ Transform OdometryORBSLAM2::computeTransform(
}
info->wordMatches.resize(oi);
info->wordInliers.resize(oi);
info->inliers = oi;
std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
for (unsigned int i = 0; i < mapPoints.size(); ++i)
{
cv::Mat pt = mapPoints[i]->GetWorldPos();
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), localTransform)));
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
}
}
}