Only update weight on rehearsal if the displacement is too high (using parameters RGBD/LinearUpdate and RGBD/AngularUpdate)

corelib/include/rtabmap/core/Memory.h

@@ -210,6 +210,8 @@ private:
 	bool _badSignaturesIgnored;
 	int _imageDecimation;
 	bool _localSpaceLinksKeptInWM;
+	float _rehearsalMaxDistance;
+	float _rehearsalMaxAngle;
 
 	int _idCount;
 	int _idMapCount;

corelib/src/Memory.cpp

@@ -68,6 +68,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_imageDecimation(Parameters::defaultMemImageDecimation()),
 	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
+	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
+	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -387,6 +389,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
 	Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
+	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
+	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -2429,7 +2433,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 	//============================================================
 	int id = signature->getLinks().begin()->first;
 	UDEBUG("Comparing with last signature (%d)...", id);
-	const Signature * sB = this->getSignature(id);
+	Signature * sB = this->_getSignature(id);
 	if(!sB)
 	{
 		UFATAL("Signature %d null?!?", id);
@@ -2441,9 +2445,36 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 	{
 		if(_incrementalMemory)
 		{
-			if(this->rehearsalMerge(id, signature->id()))
+			if(signature->getLinks().begin()->second.transform().isNull())
 			{
-				merged = id;
+				if(this->rehearsalMerge(id, signature->id()))
+				{
+					merged = id;
+				}
+			}
+			else
+			{
+				float x,y,z, roll,pitch,yaw;
+				signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				if((_rehearsalMaxDistance>0.0f && (
+						fabs(x) > _rehearsalMaxDistance ||
+						fabs(y) > _rehearsalMaxDistance ||
+						fabs(z) > _rehearsalMaxDistance)) ||
+					(_rehearsalMaxAngle>0.0f && (
+					fabs(roll) > _rehearsalMaxAngle ||
+					fabs(pitch) > _rehearsalMaxAngle ||
+					fabs(yaw) > _rehearsalMaxAngle)))
+				{
+					// if the robot has moved, transfer only weight
+					signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
+					sB->setWeight(0);
+					UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
+							signature->getWeight(), signature->id(), id, _rehearsalMaxDistance, _rehearsalMaxAngle);
+				}
+				else if(this->rehearsalMerge(id, signature->id()))
+				{
+					merged = id;
+				}
 			}
 		}
 		else
@@ -2474,7 +2505,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		}
 		UASSERT(!newS->isSaved());
 
-		UDEBUG("Rehearsal merge %d and %d", oldS->id(), newS->id());
+		UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id());
 
 		//remove mutual links
 		oldS->removeLink(newId);

corelib/src/Rtabmap.cpp

@@ -817,7 +817,7 @@ bool Rtabmap::process(const SensorData & data)
 		{
 			_optimizedPoses.erase(rehearsedId);
 		}
-		else
+		else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
 		{
 			//============================================================
 			// Minimum displacement required to add to Memory
@@ -827,14 +827,17 @@ bool Rtabmap::process(const SensorData & data)
 			{
 				float x,y,z, roll,pitch,yaw;
 				links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-				if(fabs(x) < _rgbdLinearUpdate &&
+				if((_rgbdLinearUpdate==0.0f || (
-					fabs(y) < _rgbdLinearUpdate &&
+					 fabs(x) < _rgbdLinearUpdate &&
-					fabs(z) < _rgbdLinearUpdate &&
+					 fabs(y) < _rgbdLinearUpdate &&
-					fabs(roll) < _rgbdAngularUpdate &&
+					 fabs(z) < _rgbdLinearUpdate)) &&
-					fabs(pitch) < _rgbdAngularUpdate &&
+					(_rgbdAngularUpdate==0.0f || (
-					fabs(yaw) < _rgbdAngularUpdate)
+					 fabs(roll) < _rgbdAngularUpdate &&
+					 fabs(pitch) < _rgbdAngularUpdate &&
+					 fabs(yaw) < _rgbdAngularUpdate)))
 				{
-					UWARN("Ignoring location %d because the displacement is too small!", signature->id());
+					UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+							signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
 					_memory->deleteLocation(signature->id());
 					return false;
 				}
@@ -1367,7 +1370,7 @@ bool Rtabmap::process(const SensorData & data)
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
-				UWARN("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
+				UINFO("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
 			}
 		}
 		if(!rejectedHypothesis)