Added RGBD/LinearSpeedUpdate and RGBD/AngularSpeedUpdate parameters

corelib/include/rtabmap/core/CameraInfo.h

@@ -66,6 +66,7 @@ public:
 	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -316,8 +316,10 @@ class RTABMAP_EXP Parameters
 
     // RGB-D SLAM
     RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
-    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -225,6 +225,8 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	float _rgbdLinearSpeedUpdate;
+	float _rgbdAngularSpeedUpdate;
 	float _newMapOdomChangeDistance;
 	bool _neighborLinkRefining;
 	bool _proximityByTime;

corelib/include/rtabmap/core/Statistics.h

@@ -98,6 +98,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Fast_movement,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);

corelib/src/DBReader.cpp

@@ -456,6 +456,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 				{
 					info->odomPose = pose;
 					info->odomCovariance = infMatrix.inv();
+					info->odomVelocity = velocity;
 					UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
 				}
 			}

corelib/src/Rtabmap.cpp

@@ -93,6 +93,8 @@ Rtabmap::Rtabmap() :
 	_rgbdSlamMode(Parameters::defaultRGBDEnabled()),
 	_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
+	_rgbdLinearSpeedUpdate(Parameters::defaultRGBDLinearSpeedUpdate()),
+	_rgbdAngularSpeedUpdate(Parameters::defaultRGBDAngularSpeedUpdate()),
 	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
 	_neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
 	_proximityByTime(Parameters::defaultRGBDProximityByTime()),
@@ -409,6 +411,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
+	Parameters::parse(parameters, Parameters::kRGBDLinearSpeedUpdate(), _rgbdLinearSpeedUpdate);
+	Parameters::parse(parameters, Parameters::kRGBDAngularSpeedUpdate(), _rgbdAngularSpeedUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
 	Parameters::parse(parameters, Parameters::kRGBDNeighborLinkRefining(), _neighborLinkRefining);
 	Parameters::parse(parameters, Parameters::kRGBDProximityByTime(), _proximityByTime);
@@ -436,6 +440,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
+	UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
+	UASSERT(_rgbdAngularSpeedUpdate >= 0.0f);
 
 	// By default, we create our strategies if they are not already created.
 	// If they already exists, we check the parameters if a change is requested
@@ -1008,6 +1014,7 @@ bool Rtabmap::process(
 	// Metric
 	//============================================================
 	bool smallDisplacement = false;
+	bool tooFastMovement = false;
 	std::list<int> signaturesRemoved;
 	if(_rgbdSlamMode)
 	{
@@ -1020,35 +1027,46 @@ bool Rtabmap::process(
 		{
 			_optimizedPoses.erase(rehearsedId);
 		}
-		else if(signature->getWeight() >= 0 && _rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
+		else if(signature->getWeight() >= 0)
 		{
-			//============================================================
-			// Minimum displacement required to add to Memory
-			//============================================================
-			const std::map<int, Link> & links = signature->getLinks();
-			if(links.size() && links.begin()->second.type() == Link::kNeighbor)
+			if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
 			{
-				// don't do this if there are intermediate nodes
-				const Signature * s = _memory->getSignature(links.begin()->second.to());
-				UASSERT(s!=0);
-				if(s->getWeight() >= 0)
+				//============================================================
+				// Minimum displacement required to add to Memory
+				//============================================================
+				const std::map<int, Link> & links = signature->getLinks();
+				if(links.size() && links.begin()->second.type() == Link::kNeighbor)
 				{
-					float x,y,z, roll,pitch,yaw;
-					links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
-									fabs(y) > _rgbdLinearUpdate ||
-									fabs(z) > _rgbdLinearUpdate ||
-									fabs(roll) > _rgbdAngularUpdate ||
-									fabs(pitch) > _rgbdAngularUpdate ||
-									fabs(yaw) > _rgbdAngularUpdate;
-					if(!isMoving)
+					// don't do this if there are intermediate nodes
+					const Signature * s = _memory->getSignature(links.begin()->second.to());
+					UASSERT(s!=0);
+					if(s->getWeight() >= 0)
 					{
-						// This will disable global loop closure detection, only retrieval will be done.
-						// The location will also be deleted at the end.
-						smallDisplacement = true;
+						float x,y,z, roll,pitch,yaw;
+						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
+										fabs(y) > _rgbdLinearUpdate ||
+										fabs(z) > _rgbdLinearUpdate ||
+										fabs(roll) > _rgbdAngularUpdate ||
+										fabs(pitch) > _rgbdAngularUpdate ||
+										fabs(yaw) > _rgbdAngularUpdate;
+						if(!isMoving)
+						{
+							// This will disable global loop closure detection, only retrieval will be done.
+							// The location will also be deleted at the end.
+							smallDisplacement = true;
+						}
 					}
 				}
 			}
+			if(odomVelocity.size() == 6)
+			{
+				// This will disable global loop closure detection, only retrieval will be done.
+				// The location will also be deleted at the end.
+				tooFastMovement =
+						(_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
+						(_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
+			}
 		}
 
 		// Update optimizedPoses with the newly added node
@@ -1293,8 +1311,8 @@ bool Rtabmap::process(
 	// Bayes filter update
 	//============================================================
 	int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
-	// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure
-	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
+	// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement
+	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
 	{
 		// If the working memory is empty, don't do the detection. It happens when it
 		// is the first time the detector is started (there needs some images to
@@ -1436,7 +1454,7 @@ bool Rtabmap::process(
 			}
 		} // if(_memory->getWorkingMemSize())
 	}// !isBadSignature
-	else if(!signature->isBadSignature() && smallDisplacement)
+	else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
 	{
 		_highestHypothesis = lastHighestHypothesis;
 	}
@@ -1878,7 +1896,8 @@ bool Rtabmap::process(
 			// closures if we are already localized by a global closure.
 
 			// don't do it if it is a small displacement unless the previous signature didn't have a loop closure
-			if(!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)
+			// don't do it if there is a too fast movement
+			if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
 			{
 
 				//============================================================
@@ -2404,6 +2423,7 @@ bool Rtabmap::process(
 
 			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
 			statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
+			statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
 
 			if(_publishLikelihood || _publishPdf)
 			{
@@ -2471,7 +2491,7 @@ bool Rtabmap::process(
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
 		}
-		else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
+		else if((smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
 		{
 			// Don't delete the location if a loop closure is detected
 			UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",

corelib/src/RtabmapThread.cpp

@@ -337,6 +337,17 @@ bool RtabmapThread::handleEvent(UEvent* event)
 					{
 						OdometryInfo infoCov;
 						infoCov.reg.covariance = e->info().odomCovariance;
+						if(e->info().odomVelocity.size() == 6)
+						{
+							infoCov.transform = Transform(
+									e->info().odomVelocity[0],
+									e->info().odomVelocity[1],
+									e->info().odomVelocity[2],
+									e->info().odomVelocity[3],
+									e->info().odomVelocity[4],
+									e->info().odomVelocity[5]);
+							infoCov.interval = 1.0;
+						}
 						this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 					}
 					else