Updated how odometry guess is done (adapt to time between timestamps) and Kalman filtering (using velocity)

corelib/include/rtabmap/core/Odometry.h

@@ -60,18 +60,19 @@ public:
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	const Transform & previousTransform() const {return previousTransform_;}
 
 private:
-	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0) = 0;
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
 
-	void initKalmanFilter();
+	void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
-	void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
+	void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
+	void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
 
 private:
 	int _resetCountdown;
 	bool _force3DoF;
 	bool _holonomic;
+	bool guessFromMotion_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -84,11 +85,11 @@ private:
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
-	Transform previousTransform_;
+	Transform previousVelocityTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 
-	std::vector<ParticleFilter *> filters_;
+	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
 
 protected:

corelib/include/rtabmap/core/OdometryF2F.h

@@ -46,12 +46,11 @@ public:
 	const Signature & getRefFrame() const {return refFrame_;}
 
 private:
-	virtual Transform computeTransform(SensorData & image, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
 	//Parameters:
 	int keyFrameThr_;
-	bool guessFromMotion_;
 
 	Registration * registrationPipeline_;
 	Signature refFrame_;

corelib/include/rtabmap/core/OdometryF2M.h

@@ -46,7 +46,7 @@ public:
 	const Signature & getLastFrame() const {return *lastFrame_;}
 
 private:
-	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
 
 private:
 	//Parameters

corelib/include/rtabmap/core/OdometryMono.h

@@ -43,7 +43,7 @@ public:
 	virtual void reset(const Transform & initialPose);
 
 private:
-	virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
 private:
 	//Parameters:
 	int flowWinSize_;

corelib/include/rtabmap/core/Parameters.h

@@ -362,7 +362,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01, "Maximum variance to add new points to local map.");
 
 	// Odometry Optical Flow
-	RTABMAP_PARAM(OdomF2F, KeyFrameThr,           int, 100,      "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(OdomF2F, KeyFrameThr,           int, 500,      "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
 
 	// Common registration parameters
 	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
@@ -420,7 +420,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
 	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
 	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
-	RTABMAP_PARAM(Stereo, MaxSlope,              float, 0.1,    "[Stereo/OpticalFlow = true] The maximum slope for each stereo pairs.");
 
 	RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");

corelib/include/rtabmap/core/Stereo.h

@@ -80,11 +80,9 @@ public:
 			std::vector<unsigned char> & status) const;
 
 	float epsilon() const {return epsilon_;}
-	float maxSlope() const {return maxSlope_;}
 
 private:
 	float epsilon_;
-	float maxSlope_;
 };
 
 } /* namespace rtabmap */

corelib/src/Odometry.cpp

@@ -65,6 +65,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force3DoF(Parameters::defaultRegForce3DoF()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
+		guessFromMotion_(Parameters::defaultOdomGuessMotion()),
 		_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
 		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -76,13 +77,14 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
-		previousTransform_(Transform::getIdentity()),
+		previousVelocityTransform_(Transform::getIdentity()),
 		distanceTravelled_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
+	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -99,16 +101,16 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	if(_filteringStrategy == 2)
 	{
 		// Initialize the Particle filters
-		filters_.resize(6);
+		particleFilters_.resize(6);
-		for(unsigned int i = 0; i<filters_.size(); ++i)
+		for(unsigned int i = 0; i<particleFilters_.size(); ++i)
 		{
 			if(i<3)
 			{
-				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
+				particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
 			}
 			else
 			{
-				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
+				particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
 			}
 		}
 	}
@@ -120,21 +122,21 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 
 Odometry::~Odometry()
 {
-	for(unsigned int i=0; i<filters_.size(); ++i)
+	for(unsigned int i=0; i<particleFilters_.size(); ++i)
 	{
-		delete filters_[i];
+		delete particleFilters_[i];
 	}
-	filters_.clear();
+	particleFilters_.clear();
 }
 
 void Odometry::reset(const Transform & initialPose)
 {
-	previousTransform_.setIdentity();
+	previousVelocityTransform_.setIdentity();
 	previousGroundTruthPose_.setNull();
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
-	if(_force3DoF || filters_.size())
+	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
 		initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
@@ -156,34 +158,20 @@ void Odometry::reset(const Transform & initialPose)
 			_pose = initialPose;
 		}
 
-		if(filters_.size())
+		if(particleFilters_.size())
 		{
-			UASSERT(filters_.size() == 6);
+			UASSERT(particleFilters_.size() == 6);
-			filters_[0]->init(x);
+			particleFilters_[0]->init(x);
-			filters_[1]->init(y);
+			particleFilters_[1]->init(y);
-			filters_[2]->init(z);
+			particleFilters_[2]->init(z);
-			filters_[3]->init(roll);
+			particleFilters_[3]->init(roll);
-			filters_[4]->init(pitch);
+			particleFilters_[4]->init(pitch);
-			filters_[5]->init(yaw);
+			particleFilters_[5]->init(yaw);
 		}
 
 		if(_filteringStrategy == 1)
 		{
-			if(_force3DoF)
+			initKalmanFilter(initialPose);
-			{
-				kalmanFilter_.statePost.at<float>(0) = x;
-				kalmanFilter_.statePost.at<float>(1) = y;
-				kalmanFilter_.statePost.at<float>(6) = yaw;
-			}
-			else
-			{
-				kalmanFilter_.statePost.at<float>(0) = x;
-				kalmanFilter_.statePost.at<float>(1) = y;
-				kalmanFilter_.statePost.at<float>(2) = z;
-				kalmanFilter_.statePost.at<float>(9) = roll;
-				kalmanFilter_.statePost.at<float>(10) = pitch;
-				kalmanFilter_.statePost.at<float>(11) = yaw;
-			}
 		}
 	}
 	else
@@ -208,10 +196,38 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 		return Transform();
 	}
 
-	UTimer time;
-	Transform t = this->computeTransform(data, info);
-
 	double dt = data.stamp() - previousStamp_;
+	Transform guess;
+	if(	!previousVelocityTransform_.isNull() &&
+		!previousVelocityTransform_.isIdentity())
+	{
+		if(guessFromMotion_)
+		{
+			if(_filteringStrategy == 1)
+			{
+				// use Kalman predict transform
+				float vx,vy,vz, vroll,vpitch,vyaw;
+				predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
+				guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
+			}
+			else
+			{
+				float vx,vy,vz, vroll,vpitch,vyaw;
+				previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+				guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
+			}
+		}
+		else if(_filteringStrategy == 1)
+		{
+			predictKalmanFilter(dt);
+		}
+	}
+	previousVelocityTransform_.setNull();
+	previousStamp_ = data.stamp();
+
+	UTimer time;
+	Transform t = this->computeTransform(data, guess, info);
+
 	if(info)
 	{
 		info->timeEstimation = time.ticks();
@@ -230,69 +246,84 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 		}
 	}
 
-	previousTransform_.setIdentity();
-	previousStamp_ = data.stamp();
-
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		if(_force3DoF || !_holonomic || filters_.size() || _filteringStrategy==1)
+		float vx,vy,vz, vroll,vpitch,vyaw;
-		{
+		t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
-			float x,y,z, roll,pitch,yaw;
-			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
 
+		// transform to velocity
+		if(dt)
+		{
+			vx /= dt;
+			vy /= dt;
+			vz /= dt;
+			vroll /= dt;
+			vpitch /= dt;
+			vyaw /= dt;
+		}
+
+		if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
+		{
 			if(_filteringStrategy == 1)
 			{
 				if(_pose.isIdentity())
 				{
 					// reset Kalman
+					if(t.isIdentity())
+					{
 						initKalmanFilter();
 					}
 					else
 					{
-					// Kalman filtering
+						initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
-					updateKalmanFilter(dt,x,y,z,roll,pitch,yaw);
 					}
 				}
-			else if(filters_.size())
-			{
-				// Particle filtering
-				UASSERT(filters_.size()==6);
-				if(_pose.isIdentity())
-				{
-					filters_[0]->init(x);
-					filters_[1]->init(y);
-					filters_[2]->init(z);
-					filters_[3]->init(roll);
-					filters_[4]->init(pitch);
-					filters_[5]->init(yaw);
-				}
 				else
 				{
-					x = filters_[0]->filter(x);
+					// Kalman filtering
-					y = filters_[1]->filter(y);
+					updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
-					yaw = filters_[5]->filter(yaw);
+				}
+			}
+			else if(particleFilters_.size())
+			{
+				// Particle filtering
+				UASSERT(particleFilters_.size()==6);
+				if(_pose.isIdentity())
+				{
+					particleFilters_[0]->init(vx);
+					particleFilters_[1]->init(vy);
+					particleFilters_[2]->init(vz);
+					particleFilters_[3]->init(vroll);
+					particleFilters_[4]->init(vpitch);
+					particleFilters_[5]->init(vyaw);
+				}
+				else
+				{
+					vx = particleFilters_[0]->filter(vx);
+					vy = particleFilters_[1]->filter(vy);
+					vyaw = particleFilters_[5]->filter(vyaw);
 
 					if(!_holonomic)
 					{
 						// arc trajectory around ICR
-						float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
+						float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-						if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
+						if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
 						{
-							y = tmpY;
+							vy = tmpY;
 						}
 						else
 						{
-							yaw = (atan(x/y)*2.0f-CV_PI)*-1;
+							vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
 						}
 					}
 
 					if(!_force3DoF)
 					{
-						z = filters_[2]->filter(z);
+						vz = particleFilters_[2]->filter(vz);
-						roll = filters_[3]->filter(roll);
+						vroll = particleFilters_[3]->filter(vroll);
-						pitch = filters_[4]->filter(pitch);
+						vpitch = particleFilters_[4]->filter(vpitch);
 					}
 				}
 
@@ -300,30 +331,43 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 				{
 					info->timeParticleFiltering = time.ticks();
 				}
+
+				if(_force3DoF)
+				{
+					vz = 0.0f;
+					vroll = 0.0f;
+					vpitch = 0.0f;
+				}
 			}
 			else if(!_holonomic)
 			{
 				// arc trajectory around ICR
-				float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
+				float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-				if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
+				if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
 				{
-					y = tmpY;
+					vy = tmpY;
 				}
 				else
 				{
-					yaw = (atan(x/y)*2.0f-CV_PI)*-1;
+					vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
+				}
+				if(_force3DoF)
+				{
+					vz = 0.0f;
+					vroll = 0.0f;
+					vpitch = 0.0f;
 				}
 			}
-			UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
-					uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
-					uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
-							x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
-			t = Transform(x,y,_force3DoF?0:z, _force3DoF?0:roll,_force3DoF?0:pitch,yaw);
 
+			t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
+			if(info)
+			{
 				info->transformFiltered = t;
 			}
+		}
+
+		previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
 
-		previousTransform_ = t;
 		if(info)
 		{
 			distanceTravelled_ += t.getNorm();
@@ -347,7 +391,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 	return Transform();
 }
 
-void Odometry::initKalmanFilter()
+void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
 {
 	UDEBUG("");
 	// See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
@@ -355,48 +399,91 @@ void Odometry::initKalmanFilter()
 
 	// initialize the Kalman filter
 	int nStates = 18;            // the number of states (x,y,z,x',y',z',x'',y'',z'',roll,pitch,yaw,roll',pitch',yaw',roll'',pitch'',yaw'')
-	int nMeasurements = 6;       // the number of measured states (x,y,z,roll,pitch,yaw)
+	int nMeasurements = 6;       // the number of measured states (x',y',z',roll',pitch',yaw')
 	if(_force3DoF)
 	{
 		nStates = 9;             // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
-		nMeasurements = 3;       // the number of measured states (x,y,z,roll,pitch,yaw)
+		nMeasurements = 3;       // the number of measured states (x',y',yaw')
 	}
 	int nInputs = 0;             // the number of action control
 
+	/* From viso2, measurement covariance
+	 * static const boost::array<double, 36> STANDARD_POSE_COVARIANCE =
+	{ { 0.1, 0, 0, 0, 0, 0,
+	    0, 0.1, 0, 0, 0, 0,
+	    0, 0, 0.1, 0, 0, 0,
+	    0, 0, 0, 0.17, 0, 0,
+	    0, 0, 0, 0, 0.17, 0,
+	    0, 0, 0, 0, 0, 0.17 } };
+	static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
+	{ { 0.05, 0, 0, 0, 0, 0,
+	    0, 0.05, 0, 0, 0, 0,
+	    0, 0, 0.05, 0, 0, 0,
+	    0, 0, 0, 0.09, 0, 0,
+	    0, 0, 0, 0, 0.09, 0,
+	    0, 0, 0, 0, 0, 0.09 } };
+	 */
+
+
 	kalmanFilter_.init(nStates, nMeasurements, nInputs);                 // init Kalman Filter
 	cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise));  // set process noise
 	cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise));   // set measurement noise
 	cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1));             // error covariance
 
+	float x,y,z,roll,pitch,yaw;
+	initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+
 	if(_force3DoF)
 	{
-        /* MEASUREMENT MODEL */
+        /* MEASUREMENT MODEL (velocity) */
-		//  [1 0 0 0 0 0 0 0 0]
+		//  [0 0 1 0 0 0 0 0 0]
-		//  [0 1 0 0 0 0 0 0 0]
+		//  [0 0 0 1 0 0 0 0 0]
-		//  [0 0 0 0 0 0 1 0 0]
+		//  [0 0 0 0 0 0 0 1 0]
-		kalmanFilter_.measurementMatrix.at<float>(0,0) = 1;  // x
+		kalmanFilter_.measurementMatrix.at<float>(0,2) = 1;  // x'
-		kalmanFilter_.measurementMatrix.at<float>(1,1) = 1;  // y
+		kalmanFilter_.measurementMatrix.at<float>(1,3) = 1;  // y'
-		kalmanFilter_.measurementMatrix.at<float>(2,6) = 1; // yaw
+		kalmanFilter_.measurementMatrix.at<float>(2,7) = 1; // yaw'
+
+		kalmanFilter_.statePost.at<float>(0) = x;
+		kalmanFilter_.statePost.at<float>(1) = y;
+		kalmanFilter_.statePost.at<float>(6) = yaw;
+
+		kalmanFilter_.statePost.at<float>(2) = vx;
+		kalmanFilter_.statePost.at<float>(3) = vy;
+		kalmanFilter_.statePost.at<float>(7) = vyaw;
 	}
 	else
 	{
-	    /* MEASUREMENT MODEL */
+	    /* MEASUREMENT MODEL (velocity) */
-		//  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
-		//  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0]
-		//  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]
-		//  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0]
-		//  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0]
-		//  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
+		//  [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0]
-		kalmanFilter_.measurementMatrix.at<float>(0,0) = 1;  // x
+		kalmanFilter_.measurementMatrix.at<float>(0,3) = 1;  // x'
-		kalmanFilter_.measurementMatrix.at<float>(1,1) = 1;  // y
+		kalmanFilter_.measurementMatrix.at<float>(1,4) = 1;  // y'
-		kalmanFilter_.measurementMatrix.at<float>(2,2) = 1;  // z
+		kalmanFilter_.measurementMatrix.at<float>(2,5) = 1;  // z'
-		kalmanFilter_.measurementMatrix.at<float>(3,9) = 1;  // roll
+		kalmanFilter_.measurementMatrix.at<float>(3,12) = 1; // roll'
-		kalmanFilter_.measurementMatrix.at<float>(4,10) = 1; // pitch
+		kalmanFilter_.measurementMatrix.at<float>(4,13) = 1; // pitch'
-		kalmanFilter_.measurementMatrix.at<float>(5,11) = 1; // yaw
+		kalmanFilter_.measurementMatrix.at<float>(5,14) = 1; // yaw'
+
+		kalmanFilter_.statePost.at<float>(0) = x;
+		kalmanFilter_.statePost.at<float>(1) = y;
+		kalmanFilter_.statePost.at<float>(2) = z;
+		kalmanFilter_.statePost.at<float>(9) = roll;
+		kalmanFilter_.statePost.at<float>(10) = pitch;
+		kalmanFilter_.statePost.at<float>(11) = yaw;
+
+		kalmanFilter_.statePost.at<float>(3) = vx;
+		kalmanFilter_.statePost.at<float>(4) = vy;
+		kalmanFilter_.statePost.at<float>(5) = vz;
+		kalmanFilter_.statePost.at<float>(12) = vroll;
+		kalmanFilter_.statePost.at<float>(13) = vpitch;
+		kalmanFilter_.statePost.at<float>(14) = vyaw;
 	}
 }
 
-void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw)
+void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
 {
 	// Set transition matrix with current dt
 	if(_force3DoF)
@@ -464,48 +551,57 @@ void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, flo
 		kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
 	}
 
+	// First predict, to update the internal statePre variable
+	UDEBUG("Predict");
+	const cv::Mat & prediction = kalmanFilter_.predict();
+
+	if(vx)
+		*vx = prediction.at<float>(3);                      // x'
+	if(vy)
+		*vy = prediction.at<float>(4);                      // y'
+	if(vz)
+		*vz = _force3DoF?0.0f:prediction.at<float>(5);      // z'
+	if(vroll)
+		*vroll = _force3DoF?0.0f:prediction.at<float>(12);  // roll'
+	if(vpitch)
+		*vpitch = _force3DoF?0.0f:prediction.at<float>(13); // pitch'
+	if(vyaw)
+		*vyaw = prediction.at<float>(_force3DoF?7:14);      // yaw'
+}
+
+void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
+{
 	// Set measurement to predict
 	cv::Mat measurements;
 	if(!_force3DoF)
 	{
 		measurements = cv::Mat(6,1,CV_32FC1);
-		measurements.at<float>(0) = x;     // x
+		measurements.at<float>(0) = vx;     // x'
-		measurements.at<float>(1) = y;     // y
+		measurements.at<float>(1) = vy;     // y'
-		measurements.at<float>(2) = z;     // z
+		measurements.at<float>(2) = vz;     // z'
-		measurements.at<float>(3) = roll;  // roll
+		measurements.at<float>(3) = vroll;  // roll'
-		measurements.at<float>(4) = pitch; // pitch
+		measurements.at<float>(4) = vpitch; // pitch'
-		measurements.at<float>(5) = yaw;   // yaw
+		measurements.at<float>(5) = vyaw;   // yaw'
 	}
 	else
 	{
 		measurements = cv::Mat(3,1,CV_32FC1);
-		measurements.at<float>(0) = x;     // x
+		measurements.at<float>(0) = vx;     // x'
-		measurements.at<float>(1) = y;     // y
+		measurements.at<float>(1) = vy;     // y'
-		measurements.at<float>(5) = yaw;   // yaw
+		measurements.at<float>(2) = vyaw;   // yaw',
 	}
 
-	// First predict, to update the internal statePre variable
-	UDEBUG("Predict");
-	cv::Mat prediction = kalmanFilter_.predict();
 	// The "correct" phase that is going to use the predicted value and our measurement
 	UDEBUG("Correct");
-	cv::Mat estimated = kalmanFilter_.correct(measurements);
+	const cv::Mat & estimated = kalmanFilter_.correct(measurements);
 
-	if(_force3DoF)
+
-	{
+	vx = estimated.at<float>(3);                      // x'
-		x = estimated.at<float>(0);
+	vy = estimated.at<float>(4);                      // y'
-		y = estimated.at<float>(1);
+	vz = _force3DoF?0.0f:estimated.at<float>(5);      // z'
-		yaw = estimated.at<float>(6);
+	vroll = _force3DoF?0.0f:estimated.at<float>(12);  // roll'
-	}
+	vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
-	else
+	vyaw = estimated.at<float>(_force3DoF?7:14);      // yaw'
-	{
-		x = estimated.at<float>(0);
-		y = estimated.at<float>(1);
-		z = estimated.at<float>(2);
-		roll = estimated.at<float>(9);
-		pitch = estimated.at<float>(10);
-		yaw = estimated.at<float>(11);
-	}
 }
 
 } /* namespace rtabmap */

corelib/src/OdometryF2F.cpp

@@ -37,12 +37,10 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
-	guessFromMotion_(Parameters::defaultOdomGuessMotion()),
 	motionSinceLastKeyFrame_(Transform::getIdentity())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_);
-	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
 }
 
 OdometryF2F::~OdometryF2F()
@@ -60,6 +58,7 @@ void OdometryF2F::reset(const Transform & initialPose)
 // return not null transform if odometry is correctly computed
 Transform OdometryF2F::computeTransform(
 		SensorData & data,
+		const Transform & guess,
 		OdometryInfo * info)
 {
 	UTimer timer;
@@ -84,7 +83,7 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				refFrame_,
 				newFrame,
-				guessFromMotion_&&!this->previousTransform().isNull()?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
+				!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
 				&regInfo);
 
 		if(info && this->isInfoDataFilled())
@@ -188,6 +187,7 @@ Transform OdometryF2F::computeTransform(
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;
+		info->features = refFrame_.sensorData().keypoints().size();
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

corelib/src/OdometryF2M.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/calib3d/calib3d.hpp>
 #include <rtabmap/core/OdometryF2M.h>
@@ -167,6 +168,7 @@ void OdometryF2M::reset(const Transform & initialPose)
 // return not null transform if odometry is correctly computed
 Transform OdometryF2M::computeTransform(
 		SensorData & data,
+		const Transform & guess,
 		OdometryInfo * info)
 {
 	UTimer timer;
@@ -188,9 +190,12 @@ Transform OdometryF2M::computeTransform(
 	{
 		if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
 		{
-			Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
 			Signature tmpMap = *map_;
-			Transform transform = regVis_->computeTransformationMod(tmpMap, *lastFrame_, guess, &regInfo);
+			Transform transform = regVis_->computeTransformationMod(
+					tmpMap,
+					*lastFrame_,
+					guess.isNull()?Transform():this->getPose()*guess,
+					&regInfo);
 
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
 

corelib/src/OdometryMono.cpp

@@ -170,7 +170,7 @@ void OdometryMono::reset(const Transform & initialPose)
 	keyFramePoses_.clear();
 }
 
-Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
+Transform OdometryMono::computeTransform(SensorData & data, const Transform & guess, OdometryInfo * info)
 {
 	Transform output;
 
@@ -229,14 +229,14 @@ Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
 				if((int)newS->getWords().size() > minInliers_)
 				{
 					cv::Mat K = cameraModel.K();
-					Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
+					Transform pnpGuess = ((this->getPose() * (guess.isNull()?Transform::getIdentity():guess)) * cameraModel.localTransform()).inverse();
 					cv::Mat R = (cv::Mat_<double>(3,3) <<
-							(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+							(double)pnpGuess.r11(), (double)pnpGuess.r12(), (double)pnpGuess.r13(),
-							(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+							(double)pnpGuess.r21(), (double)pnpGuess.r22(), (double)pnpGuess.r23(),
-							(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+							(double)pnpGuess.r31(), (double)pnpGuess.r32(), (double)pnpGuess.r33());
 					cv::Mat rvec(1,3, CV_64FC1);
 					cv::Rodrigues(R, rvec);
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)pnpGuess.x(), (double)pnpGuess.y(), (double)pnpGuess.z());
 
 					std::vector<cv::Point3f> objectPoints;
 					std::vector<cv::Point2f> imagePoints;

corelib/src/Stereo.cpp

@@ -94,8 +94,7 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 
 StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
 		Stereo(parameters),
-		epsilon_(Parameters::defaultStereoEps()),
+		epsilon_(Parameters::defaultStereoEps())
-		maxSlope_(Parameters::defaultStereoMaxSlope())
 {
 	this->parseParameters(parameters);
 }
@@ -104,7 +103,6 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
 {
 	Stereo::parseParameters(parameters);
 	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
-	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), maxSlope_);
 }
 
 
@@ -135,9 +133,7 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		if(status[i]!=0)
 		{
 			float disparity = leftCorners[i].x - rightCorners[i].x;
-			float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
+			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()))
-			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()) ||
-			   (maxSlope_ > 0.0f && fabs(leftCorners[i].y-rightCorners[i].y) > 1.0f && slope > maxSlope_))
 			{
 				status[i] = 0;
 			}

guilib/src/CloudViewer.cpp

@@ -128,6 +128,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 	// the "Invalid drawable" warning when the view is not visible.
 	//_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
 	this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
+	_visualizer->getInteractorStyle()->GetInteractor()->SetDesiredUpdateRate(5.0);
 
 	_visualizer->setCameraPosition(
 				-1, 0, 0,

guilib/src/ImageView.cpp

@@ -369,7 +369,6 @@ void ImageView::setGraphicsViewMode(bool on)
 		{
 			_imageItem = _graphicsView->scene()->addPixmap(_image);
 			_imageItem->setVisible(_showImage->isChecked());
-			_showImage->setEnabled(true);
 		}
 
 		if(_imageDepthItem)
@@ -380,7 +379,6 @@ void ImageView::setGraphicsViewMode(bool on)
 		{
 			_imageDepthItem = _graphicsView->scene()->addPixmap(_imageDepth);
 			_imageDepthItem->setVisible(_showImageDepth->isChecked());
-			_showImageDepth->setEnabled(true);
 		}
 		this->updateOpacity();
 
@@ -725,7 +723,6 @@ void ImageView::setImage(const QImage & image)
 		{
 			_imageItem = _graphicsView->scene()->addPixmap(_image);
 			_imageItem->setVisible(_showImage->isChecked());
-			_showImage->setEnabled(true);
 			this->updateOpacity();
 		}
 	}
@@ -761,7 +758,6 @@ void ImageView::setImageDepth(const QImage & imageDepth)
 		{
 			_imageDepthItem = _graphicsView->scene()->addPixmap(_imageDepth);
 			_imageDepthItem->setVisible(_showImageDepth->isChecked());
-			_showImageDepth->setEnabled(true);
 			this->updateOpacity();
 		}
 	}
@@ -879,7 +875,6 @@ void ImageView::clear()
 		_graphicsView->scene()->removeItem(_imageItem);
 		delete _imageItem;
 		_imageItem = 0;
-		_showImage->setEnabled(false);
 	}
 	_image = QPixmap();
 
@@ -888,7 +883,6 @@ void ImageView::clear()
 		_graphicsView->scene()->removeItem(_imageDepthItem);
 		delete _imageDepthItem;
 		_imageDepthItem = 0;
-		_showImageDepth->setEnabled(false);
 	}
 	_imageDepth = QPixmap();
 

guilib/src/MainWindow.cpp

@@ -1789,7 +1789,9 @@ void MainWindow::updateMapCloud(
 		_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
 	}
 	cv::Mat map8U;
-	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
+	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) &&
+			((_gridLocalMaps.size() && !_preferencesDialog->isGridMapFrom3DCloud()) ||
+			 (_projectionLocalMaps.size() && _preferencesDialog->isGridMapFrom3DCloud())))
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();

guilib/src/PreferencesDialog.cpp

@@ -756,7 +756,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereo_ssd->setObjectName(Parameters::kStereoSSD().c_str());
 	_ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str());
 	_ui->stereo_opticalFlow->setObjectName(Parameters::kStereoOpticalFlow().c_str());
-	_ui->stereo_maxSlope->setObjectName(Parameters::kStereoMaxSlope().c_str());
 
 	//StereoBM
 	_ui->stereobm_blockSize->setObjectName(Parameters::kStereoBMBlockSize().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>686</width>
+            <width>681</width>
-            <height>1990</height>
+            <height>2010</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>16</number>
+                <number>18</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -9601,55 +9601,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                        <string/>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_33" columnstretch="0,1">
-                       <item row="10" column="1">
-                        <widget class="QLabel" name="label_215">
-                         <property name="text">
-                          <string>[OpticalFlow = true] The maximum slope for each stereo pairs.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
-                        <widget class="QLabel" name="label_206">
-                         <property name="text">
-                          <string>Maximum iterations.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="8" column="0">
-                        <widget class="QCheckBox" name="stereo_ssd">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="0">
-                        <widget class="QSpinBox" name="stereo_winHeight">
-                         <property name="minimum">
-                          <number>3</number>
-                         </property>
-                         <property name="maximum">
-                          <number>999999</number>
-                         </property>
-                         <property name="singleStep">
-                          <number>1</number>
-                         </property>
-                         <property name="value">
-                          <number>21</number>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="1">
                         <widget class="QLabel" name="label_284">
                          <property name="text">
@@ -9705,6 +9656,58 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="5" column="0">
+                        <widget class="QSpinBox" name="stereo_minDisparity">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>0</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QLabel" name="label_206">
+                         <property name="text">
+                          <string>Maximum iterations.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QCheckBox" name="stereo_ssd">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QSpinBox" name="stereo_winHeight">
+                         <property name="minimum">
+                          <number>3</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>21</number>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="4" column="0">
                         <widget class="QSpinBox" name="stereo_iterations">
                          <property name="minimum">
@@ -9837,22 +9840,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
-                        <widget class="QSpinBox" name="stereo_minDisparity">
-                         <property name="minimum">
-                          <number>0</number>
-                         </property>
-                         <property name="maximum">
-                          <number>999999</number>
-                         </property>
-                         <property name="singleStep">
-                          <number>1</number>
-                         </property>
-                         <property name="value">
-                          <number>0</number>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="7" column="0">
                         <widget class="QCheckBox" name="stereo_opticalFlow">
                          <property name="text">
@@ -9860,25 +9847,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="0">
-                        <widget class="QDoubleSpinBox" name="stereo_maxSlope">
-                         <property name="decimals">
-                          <number>3</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.001000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.100000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>