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

Handling intial odometry pose in all odometry approaches. For VIO approaches, gravity initialization is handled too. (#298)

Browse Source
This commit is contained in:
matlabbe
2018-07-19 14:10:36 -04:00
parent 9ae47b79f9
commit 173bd49a26
12 changed files with 124 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,6 +55,8 @@ private:
IMU lastImu_; IMU lastImu_;
ParametersMap parameters_; ParametersMap parameters_;
Transform flipXY_; Transform flipXY_;
Transform previousPose_;
bool initGravity_;
#endif #endif
}; };

3
corelib/include/rtabmap/core/OdometryORBSLAM2.h
View File

@@ -54,8 +54,9 @@ private:
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
ORBSLAM2System * orbslam2_; ORBSLAM2System * orbslam2_;
bool firstFrame_; bool firstFrame_;
#endif
Transform originLocalTransform_; Transform originLocalTransform_;
Transform previousPose_;
#endif
}; };

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

@@ -59,6 +59,8 @@ private:
ParametersMap okvisParameters_; ParametersMap okvisParameters_;
IMU lastImu_; // only used for initialization IMU lastImu_; // only used for initialization
int imagesProcessed_; int imagesProcessed_;
Transform previousPose_;
bool initGravity_;
}; };
} }

21
corelib/src/Memory.cpp
View File

@@ -2538,7 +2538,26 @@ Transform Memory::computeTransform(
{ {
int id = iter->first; int id = iter->first;
const Signature * s = this->getSignature(id); const Signature * s = this->getSignature(id);
CameraModel model = s->sensorData().cameraModels()[0]; CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
}
bundleModels.insert(std::make_pair(id, model)); bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse(); Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid()) if(iter->second.isValid())

4
corelib/src/Odometry.cpp
View File

@@ -297,7 +297,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
R(0,0), R(0,1), R(0,2), 0, R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0, R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), coefficients.values.at(3)); R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
_pose *= rotation; this->reset(rotation);
success = true; success = true;
} }
} }
@@ -316,7 +316,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform(); Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()))) if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
{ {
if(guessFromMotion_) if(guessFromMotion_ && !data.imageRaw().empty())
{ {
UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str()); UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
} }

2
corelib/src/OdometryF2M.cpp
View File

@@ -335,7 +335,7 @@ Transform OdometryF2M::computeTransform(
} }
else else
{ {
UFATAL("no valid camera model!"); UFATAL("no valid camera model to do odometry bundle adjustment!");
} }
bundleModels.insert(std::make_pair(lastFrame_->id(), model)); bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse(); Transform invLocalTransform = model.localTransform().inverse();

53
corelib/src/OdometryMSCKF.cpp
View File

@@ -747,7 +747,9 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
imageProcessor_(0), imageProcessor_(0),
msckf_(0), msckf_(0),
parameters_(parameters), parameters_(parameters),
flipXY_(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0) flipXY_(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0),
previousPose_(Transform::getIdentity()),
initGravity_(false)
#endif #endif
{ {
} }
@@ -771,17 +773,22 @@ void OdometryMSCKF::reset(const Transform & initialPose)
{ {
Odometry::reset(initialPose); Odometry::reset(initialPose);
#ifdef RTABMAP_MSCKF_VIO #ifdef RTABMAP_MSCKF_VIO
if(imageProcessor_) if(!initGravity_)
{ {
delete imageProcessor_; if(imageProcessor_)
imageProcessor_ = 0; {
delete imageProcessor_;
imageProcessor_ = 0;
}
if(msckf_)
{
delete msckf_;
msckf_ = 0;
}
lastImu_ = IMU();
previousPose_.setIdentity();
} }
if(msckf_) initGravity_ = false;
{
delete msckf_;
msckf_ = 0;
}
lastImu_ = IMU();
#endif #endif
} }
@@ -898,10 +905,24 @@ Transform OdometryMSCKF::computeTransform(
if(!p.isNull()) if(!p.isNull())
{ {
// make it incremental // pose in rtabmap/ros coordinates
p = flipXY_*p*lastImu_.localTransform(); p = flipXY_*p*lastImu_.localTransform();
Transform invCurrentPose = this->getPose().inverse();
t = invCurrentPose*p; if(this->getPose().rotation().isIdentity())
{
initGravity_ = true;
this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity())
{
previousPose_ = p;
}
// make it incremental
Transform previousPoseInv = previousPose_.inverse();
t = previousPoseInv*p;
previousPose_ = p;
if(info) if(info)
{ {
@@ -915,7 +936,7 @@ Transform OdometryMSCKF::computeTransform(
cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0), cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0),
twistCov.at<double>(1, 1), twistCov.at<double>(1, 1),
twistCov.at<double>(2, 2)); twistCov.at<double>(2, 2));
cv::Matx31f covBaseFrame = cv::Matx33f(invCurrentPose.rotationMatrix()) * covWorldFrame; cv::Matx31f covBaseFrame = cv::Matx33f(previousPoseInv.rotationMatrix()) * covWorldFrame;
// we set only diagonal values as there is an issue with g2o and off-diagonal values // we set only diagonal values as there is an issue with g2o and off-diagonal values
info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0; info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0;
info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0; info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0;
@@ -928,7 +949,7 @@ Transform OdometryMSCKF::computeTransform(
{ {
if(localMap.get() && localMap->size()) if(localMap.get() && localMap->size())
{ {
Eigen::Affine3f flip = flipXY_.toEigen3f(); Eigen::Affine3f flip = (this->getPose()*previousPoseInv*flipXY_).toEigen3f();
for(unsigned int i=0; i<localMap->size(); ++i) for(unsigned int i=0; i<localMap->size(); ++i)
{ {
pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip); pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip);
@@ -942,10 +963,12 @@ Transform OdometryMSCKF::computeTransform(
float fy = data.stereoCameraModel().left().fy(); float fy = data.stereoCameraModel().left().fy();
float cx = data.stereoCameraModel().left().cx(); float cx = data.stereoCameraModel().left().cx();
float cy = data.stereoCameraModel().left().cy(); float cy = data.stereoCameraModel().left().cy();
info->reg.inliersIDs.resize(measurements->features.size());
for(unsigned int i=0; i<measurements->features.size(); ++i) for(unsigned int i=0; i<measurements->features.size(); ++i)
{ {
info->newCorners[i].x = measurements->features[i].u0*fx+cx; info->newCorners[i].x = measurements->features[i].u0*fx+cx;
info->newCorners[i].y = measurements->features[i].v0*fy+cy; info->newCorners[i].y = measurements->features[i].v0*fy+cy;
info->reg.inliersIDs[i] = i;
} }
} }
} }

29
corelib/src/OdometryORBSLAM2.cpp
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h" #include "rtabmap/utilite/UDirectory.h"
#include <pcl/common/transforms.h>
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
#include <System.h> #include <System.h>
@@ -813,7 +814,8 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
, ,
orbslam2_(0), orbslam2_(0),
firstFrame_(true) firstFrame_(true),
previousPose_(Transform::getIdentity())
#endif #endif
{ {
#ifdef RTABMAP_ORB_SLAM2 #ifdef RTABMAP_ORB_SLAM2
@@ -841,6 +843,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
} }
firstFrame_ = true; firstFrame_ = true;
originLocalTransform_.setNull(); originLocalTransform_.setNull();
previousPose_.setIdentity();
#endif #endif
} }
@@ -908,23 +911,29 @@ Transform OdometryORBSLAM2::computeTransform(
Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp()); Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
} }
Transform previousPoseInv = previousPose_.inverse();
if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST) if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
{ {
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
} }
else if(Tcw.cols == 4 && Tcw.rows == 4) else if(Tcw.cols == 4 && Tcw.rows == 4)
{ {
t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4))); Transform p = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull()) if(!p.isNull())
{ {
if(originLocalTransform_.isNull()) if(!localTransform.isNull())
{ {
originLocalTransform_ = localTransform; if(originLocalTransform_.isNull())
{
originLocalTransform_ = localTransform;
}
// transform in base frame
p = originLocalTransform_ * p.inverse() * localTransform.inverse();
} }
t = originLocalTransform_ * t.inverse() * localTransform.inverse(); t = previousPoseInv*p;
t = this->getPose().inverse() * t;
} }
previousPose_ = p;
if(firstFrame_) if(firstFrame_)
{ {
@@ -1004,10 +1013,12 @@ Transform OdometryORBSLAM2::computeTransform(
info->reg.matches = oi; info->reg.matches = oi;
std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints(); std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
for (unsigned int i = 0; i < mapPoints.size(); ++i) for (unsigned int i = 0; i < mapPoints.size(); ++i)
{ {
cv::Mat pt = mapPoints[i]->GetWorldPos(); cv::Point3f pt(mapPoints[i]->GetWorldPos());
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_))); pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt.x, pt.y, pt.z), fixRot);
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
} }
} }
} }

40
corelib/src/OdometryOkvis.cpp
View File

@@ -133,7 +133,9 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
okvisEstimator_(0), okvisEstimator_(0),
#endif #endif
okvisParameters_(parameters), okvisParameters_(parameters),
imagesProcessed_(0) imagesProcessed_(0),
previousPose_(Transform::getIdentity()),
initGravity_(false)
{ {
#ifdef RTABMAP_OKVIS #ifdef RTABMAP_OKVIS
Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_); Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
@@ -160,17 +162,22 @@ void OdometryOkvis::reset(const Transform & initialPose)
{ {
Odometry::reset(initialPose); Odometry::reset(initialPose);
#ifdef RTABMAP_OKVIS #ifdef RTABMAP_OKVIS
if(okvisEstimator_) if(!initGravity_)
{ {
delete okvisEstimator_; if(okvisEstimator_)
okvisEstimator_ = 0; {
} delete okvisEstimator_;
lastImu_ = IMU(); okvisEstimator_ = 0;
}
lastImu_ = IMU();
imagesProcessed_ = 0;
previousPose_.setIdentity();
delete okvisCallbackHandler_; delete okvisCallbackHandler_;
okvisCallbackHandler_ = new OkvisCallbackHandler(); okvisCallbackHandler_ = new OkvisCallbackHandler();
}
initGravity_ = false;
#endif #endif
imagesProcessed_ = 0;
} }
// return not null transform if odometry is correctly computed // return not null transform if odometry is correctly computed
@@ -452,8 +459,21 @@ Transform OdometryOkvis::computeTransform(
if(!p.isNull()) if(!p.isNull())
{ {
p = fixPos * p * fixRot; p = fixPos * p * fixRot;
if(this->getPose().rotation().isIdentity())
{
initGravity_ = true;
this->reset(this->getPose()*p.rotation());
}
if(previousPose_.isIdentity())
{
previousPose_ = p;
}
// make it incremental // make it incremental
t = this->getPose().inverse()*p; t = previousPose_.inverse()*p;
previousPose_ = p;
if(info) if(info)
{ {

2
corelib/src/Rtabmap.cpp
View File

@@ -2361,7 +2361,7 @@ bool Rtabmap::process(
} }
if(maxLinearLink) if(maxLinearLink)
{ {
UINFO("Max optimization error = %f m (link %d->%d, var=%f, %f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); UINFO("Max optimization error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
float stddev = sqrt(maxLinearLink->transVariance()); float stddev = sqrt(maxLinearLink->transVariance());
maxLinearErrorRatio = maxLinearError/stddev; maxLinearErrorRatio = maxLinearError/stddev;

4
guilib/src/MainWindow.cpp
View File

@@ -1371,7 +1371,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); _ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
} }
if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) if( odom.info().type == (int)Odometry::kTypeF2M ||
odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
odom.info().type == (int)Odometry::kTypeMSCKF)
{ {
if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown())
{ {

4
guilib/src/OdometryViewer.cpp
View File

@@ -437,7 +437,9 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw())); imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
} }
if(odom.info().type == Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM2) if( odom.info().type == Odometry::kTypeF2M ||
odom.info().type == (int)Odometry::kTypeORBSLAM2 ||
odom.info().type == (int)Odometry::kTypeMSCKF)
{ {
if(imageView_->isFeaturesShown()) if(imageView_->isFeaturesShown())
{ {