0.18.2: Added Odom/GuessSmoothingDelay parameter

corelib/include/rtabmap/core/Odometry.h

@@ -70,7 +70,8 @@ public:
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
+	RTABMAP_DEPRECATED(const Transform & previousVelocityTransform() const, "Use getVelocityGuess() instead.");
+	const Transform & getVelocityGuess() const {return velocityGuess_;}
 	double previousStamp() const {return previousStamp_;}
 	unsigned int framesProcessed() const {return framesProcessed_;}
 	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
@@ -87,6 +88,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
+	bool guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -103,7 +105,8 @@ private:
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
-	Transform previousVelocityTransform_;
+	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
+	Transform velocityGuess_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 	unsigned int framesProcessed_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -106,6 +106,7 @@ public:
 	Transform transform;
 	Transform transformFiltered;
 	Transform transformGroundTruth;
+	Transform guessVelocity;
 	float distanceTravelled;
 	int memoryUsage; //MB
 

corelib/include/rtabmap/core/Parameters.h

@@ -413,6 +413,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
     RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
     RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
+    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 1.0,   uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate.", kOdomFilteringStrategy().c_str()));
     RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] 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(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");

corelib/src/Odometry.cpp

@@ -101,6 +101,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_force3DoF(Parameters::defaultRegForce3DoF()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
 		guessFromMotion_(Parameters::defaultOdomGuessMotion()),
+		guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
 		_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
 		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -125,6 +126,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
+	Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -182,7 +184,8 @@ Odometry::~Odometry()
 void Odometry::reset(const Transform & initialPose)
 {
 	UASSERT(!initialPose.isNull());
-	previousVelocityTransform_.setNull();
+	previousVelocities_.clear();
+	velocityGuess_.setNull();
 	previousGroundTruthPose_.setNull();
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
@@ -232,6 +235,37 @@ void Odometry::reset(const Transform & initialPose)
 	}
 }
 
+const Transform & Odometry::previousVelocityTransform() const
+{
+	return getVelocityGuess();
+}
+
+Transform getMeanVelocity(const std::list<std::pair<std::vector<float>, double> > & transforms)
+{
+	if(transforms.size())
+	{
+		float tvx=0.0f,tvy=0.0f,tvz=0.0f, tvroll=0.0f,tvpitch=0.0f,tvyaw=0.0f;
+		for(std::list<std::pair<std::vector<float>, double> >::const_iterator iter=transforms.begin(); iter!=transforms.end(); ++iter)
+		{
+			UASSERT(iter->first.size() == 6);
+			tvx+=iter->first[0];
+			tvy+=iter->first[1];
+			tvz+=iter->first[2];
+			tvroll+=iter->first[3];
+			tvpitch+=iter->first[4];
+			tvyaw+=iter->first[5];
+		}
+		tvx/=float(transforms.size());
+		tvy/=float(transforms.size());
+		tvz/=float(transforms.size());
+		tvroll/=float(transforms.size());
+		tvpitch/=float(transforms.size());
+		tvyaw/=float(transforms.size());
+		return Transform(tvx, tvy, tvz, tvroll, tvpitch, tvyaw);
+	}
+	return Transform();
+}
+
 Transform Odometry::process(SensorData & data, OdometryInfo * info)
 {
 	return process(data, Transform(), info);
@@ -313,21 +347,22 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
-	Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
-	if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
+	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
+	if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
 	{
 		if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
 		{
-			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, velocityGuess_.prettyPrint().c_str());
 		}
 		else if(_filteringStrategy==1)
 		{
-			UERROR("Kalman filtering is enabled but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
+			UERROR("Kalman filtering is enabled but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
 		}
 		dt=0;
-		previousVelocityTransform_.setNull();
+		previousVelocities_.clear();
+		velocityGuess_.setNull();
 	}
-	if(!previousVelocityTransform_.isNull())
+	if(!velocityGuess_.isNull())
 	{
 		if(guessFromMotion_)
 		{
@@ -341,7 +376,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			else
 			{
 				float vx,vy,vz, vroll,vpitch,vyaw;
-				previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+				velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 				guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
 			}
 		}
@@ -460,7 +495,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		{
 			if(_filteringStrategy == 1)
 			{
-				if(previousVelocityTransform_.isNull())
+				if(velocityGuess_.isNull())
 				{
 					// reset Kalman
 					if(dt)
@@ -482,7 +517,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			{
 				// Particle filtering
 				UASSERT(particleFilters_.size()==6);
-				if(previousVelocityTransform_.isNull())
+				if(velocityGuess_.isNull())
 				{
 					particleFilters_[0]->init(vx);
 					particleFilters_[1]->init(vy);
@@ -564,17 +599,43 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 
 		previousStamp_ = data.stamp();
-		previousVelocityTransform_.setNull();
 
 		if(dt)
 		{
-			previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
+			if(dt >=guessSmoothingDelay_/2.0 || particleFilters_.size() || _filteringStrategy==1)
+			{
+				velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
+				previousVelocities_.clear();
+			}
+			else
+			{
+				// smooth velocity estimation over the past X seconds
+				std::vector<float> v(6);
+				v[0] = vx;
+				v[1] = vy;
+				v[2] = vz;
+				v[3] = vroll;
+				v[4] = vpitch;
+				v[5] = vyaw;
+				previousVelocities_.push_back(std::make_pair(v, data.stamp()));
+				while(previousVelocities_.size() > 1 && previousVelocities_.front().second < previousVelocities_.back().second-guessSmoothingDelay_)
+				{
+					previousVelocities_.pop_front();
+				}
+				velocityGuess_ = getMeanVelocity(previousVelocities_);
+			}
+		}
+		else
+		{
+			previousVelocities_.clear();
+			velocityGuess_.setNull();
 		}
 
 		if(info)
 		{
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
+			info->guessVelocity = velocityGuess_;
 		}
 		++framesProcessed_;
 
@@ -592,7 +653,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
-	previousVelocityTransform_.setNull();
+	previousVelocities_.clear();
+	velocityGuess_.setNull();
 	previousStamp_ = 0;
 
 	return Transform();

corelib/src/odometry/OdometryF2M.cpp

@@ -466,7 +466,14 @@ Transform OdometryF2M::computeTransform(
 					}
 					if(!regInfo.rejectedMsg.empty())
 					{
-						UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
+						if(guess.isNull())
+						{
+							UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
+						}
+						else
+						{
+							UWARN("Registration failed: \"%s\" (guess=%s)", regInfo.rejectedMsg.c_str(), guess.prettyPrint().c_str());
+						}
 					}
 					else
 					{