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

corelib/include/rtabmap/core/OdometryMSCKF.h

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

corelib/include/rtabmap/core/OdometryORBSLAM2.h

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

corelib/include/rtabmap/core/OdometryOkvis.h

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

corelib/src/Memory.cpp

@@ -2538,7 +2538,26 @@ Transform Memory::computeTransform(
 				{
 					int id = iter->first;
 					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));
 					Transform invLocalTransform = model.localTransform().inverse();
 					if(iter->second.isValid())

corelib/src/Odometry.cpp

@@ -297,7 +297,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 							R(0,0), R(0,1), R(0,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));
-					_pose *= rotation;
+					this->reset(rotation);
 					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();
 	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());
 		}

corelib/src/OdometryF2M.cpp

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

corelib/src/OdometryMSCKF.cpp

@@ -747,7 +747,9 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 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
 {
 }
@@ -771,17 +773,22 @@ void OdometryMSCKF::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 #ifdef RTABMAP_MSCKF_VIO
-	if(imageProcessor_)
+	if(!initGravity_)
 	{
-		delete imageProcessor_;
-		imageProcessor_ = 0;
+		if(imageProcessor_)
+		{
+			delete imageProcessor_;
+			imageProcessor_ = 0;
+		}
+		if(msckf_)
+		{
+			delete msckf_;
+			msckf_ = 0;
+		}
+		lastImu_ = IMU();
+		previousPose_.setIdentity();
 	}
-	if(msckf_)
-	{
-		delete msckf_;
-		msckf_ = 0;
-	}
-	lastImu_ = IMU();
+	initGravity_ = false;
 #endif
 }
 
@@ -898,10 +905,24 @@ Transform OdometryMSCKF::computeTransform(
 
 			if(!p.isNull())
 			{
-				// make it incremental
+				// pose in rtabmap/ros coordinates
 				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)
 				{
@@ -915,7 +936,7 @@ Transform OdometryMSCKF::computeTransform(
 					cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0),
 											  twistCov.at<double>(1, 1),
 											  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
 					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;
@@ -928,7 +949,7 @@ Transform OdometryMSCKF::computeTransform(
 					{
 						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)
 							{
 								pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip);
@@ -942,10 +963,12 @@ Transform OdometryMSCKF::computeTransform(
 							float fy = data.stereoCameraModel().left().fy();
 							float cx = data.stereoCameraModel().left().cx();
 							float cy = data.stereoCameraModel().left().cy();
+							info->reg.inliersIDs.resize(measurements->features.size());
 							for(unsigned int i=0; i<measurements->features.size(); ++i)
 							{
 								info->newCorners[i].x = measurements->features[i].u0*fx+cx;
 								info->newCorners[i].y = measurements->features[i].v0*fy+cy;
+								info->reg.inliersIDs[i] = i;
 							}
 						}
 					}

corelib/src/OdometryORBSLAM2.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UDirectory.h"
+#include <pcl/common/transforms.h>
 
 #ifdef RTABMAP_ORB_SLAM2
 #include <System.h>
@@ -813,7 +814,8 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
 #ifdef RTABMAP_ORB_SLAM2
     ,
 	orbslam2_(0),
-	firstFrame_(true)
+	firstFrame_(true),
+	previousPose_(Transform::getIdentity())
 #endif
 {
 #ifdef RTABMAP_ORB_SLAM2
@@ -841,6 +843,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
 	}
 	firstFrame_ = true;
 	originLocalTransform_.setNull();
+	previousPose_.setIdentity();
 #endif
 }
 
@@ -908,23 +911,29 @@ Transform OdometryORBSLAM2::computeTransform(
 		Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
 	}
 
+	Transform previousPoseInv = previousPose_.inverse();
 	if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
 	{
 		covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
 	}
 	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 = this->getPose().inverse() * t;
+			t = previousPoseInv*p;
 		}
+		previousPose_ = p;
 
 		if(firstFrame_)
 		{
@@ -1004,10 +1013,12 @@ Transform OdometryORBSLAM2::computeTransform(
 			info->reg.matches = oi;
 
 			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)
 			{
-				cv::Mat pt = mapPoints[i]->GetWorldPos();
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
+				cv::Point3f pt(mapPoints[i]->GetWorldPos());
+				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)));
 			}
 		}
 	}

corelib/src/OdometryOkvis.cpp

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

corelib/src/Rtabmap.cpp

@@ -2361,7 +2361,7 @@ bool Rtabmap::process(
 				}
 				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());
 					maxLinearErrorRatio = maxLinearError/stddev;