Updated rehearsal behavior when there are intermediate nodes and RGBD/LinearUpdate,RGBD/AngularUpdate are set.

corelib/include/rtabmap/core/Link.h

@@ -118,25 +118,29 @@ public:
 		infMatrix_.at<double>(5,5) = 1.0/rotVariance;
 	}
 
-	Link merge(const Link & link) const
+	Link merge(const Link & link, Type outputType) const
 	{
 		UASSERT(to_ == link.from());
-		UASSERT(type_ == link.type());
-		UASSERT(!transform_.isNull());
-		UASSERT(!link.transform().isNull());
+		UASSERT(outputType != Link::kUndef);
+		UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
 		UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
 		UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
 		return Link(
 				from_,
 				link.to(),
-				type_,
-				transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
-				infMatrix_ + link.infMatrix());
+				outputType,
+				transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
+				transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
 	}
 
 	Link inverse() const
 	{
-		return Link(to_, from_, type_, transform_.inverse(), infMatrix_);
+		return Link(
+				to_,
+				from_,
+				type_,
+				transform_.isNull()?Transform():transform_.inverse(),
+				transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
 	}
 
 private:

corelib/include/rtabmap/core/Parameters.h

@@ -286,8 +286,8 @@ class RTABMAP_EXP Parameters
 	// RGB-D SLAM
 	RTABMAP_PARAM(RGBD, Enabled,           bool, true, 	"");
 	RTABMAP_PARAM(RGBD, PoseScanMatching,  bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
-	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	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 mode 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, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");

corelib/include/rtabmap/core/Statistics.h

@@ -83,6 +83,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
+	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 

corelib/src/Memory.cpp

@@ -420,6 +420,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
 	UASSERT(_imageDecimation >= 1);
+	UASSERT(_rehearsalMaxDistance >= 0.0f);
+	UASSERT(_rehearsalMaxAngle >= 0.0f);
 
 	// SLAM mode vs Localization mode
 	iter = parameters.find(Parameters::kMemIncrementalMemory());
@@ -3091,7 +3093,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 	}
 
 	//============================================================
-	// Compare with the last (not null)
+	// Compare with the last (not intermediate node)
 	//============================================================
 	Signature * sB = 0;
 	for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
@@ -3116,55 +3118,9 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 		{
 			if(_incrementalMemory)
 			{
-				if(signature->hasLink(id))
+				if(this->rehearsalMerge(id, signature->id()))
 				{
-					if(signature->getLinks().begin()->second.transform().isNull())
-					{
-						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(_rehearsalWeightIgnoredWhileMoving)
-							{
-								UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad",
-										_rehearsalMaxDistance, _rehearsalMaxAngle);
-							}
-							else
-							{
-								// if the robot has moved, increase only weight of the new one
-								signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
-								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(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
-							}
-						}
-						else if(this->rehearsalMerge(id, signature->id()))
-						{
-							merged = id;
-						}
-					}
-				}
-				else
-				{
-					// cannot merge not neighbor signatures, just update weight
-					signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
-					sB->setWeight(0);
-					UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.",
-							signature->getWeight(), signature->id(), sB->id());
+					merged = id;
 				}
 			}
 			else
@@ -3175,6 +3131,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 
 		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
 		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
+		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0);
 		UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
 	}
 	else
@@ -3202,65 +3159,122 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 
 		UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id());
 
-		//remove mutual links
-		oldS->removeLink(newId);
-		newS->removeLink(oldId);
-
-		if(_idUpdatedToNewOneRehearsal)
+		bool fullMerge;
+		bool intermediateMerge = false;
+		if(!newS->getLinks().begin()->second.transform().isNull())
 		{
-			// redirect neighbor links
-			const std::map<int, Link> & links = oldS->getLinks();
-			for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+			// we are in metric SLAM mode:
+			// 1) Normal merge if not moving AND has direct link
+			// 2) Transform to intermediate node (weight = -1) if not moving AND hasn't direct link.
+			float x,y,z, roll,pitch,yaw;
+			newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			bool isMoving = fabs(x) > _rehearsalMaxDistance ||
+							fabs(y) > _rehearsalMaxDistance ||
+							fabs(z) > _rehearsalMaxDistance ||
+							fabs(roll) > _rehearsalMaxAngle ||
+							fabs(pitch) > _rehearsalMaxAngle ||
+							fabs(yaw) > _rehearsalMaxAngle;
+			if(isMoving && _rehearsalWeightIgnoredWhileMoving)
 			{
-				Link link = iter->second;
-				link.setFrom(newS->id());
-
-				Signature * s = this->_getSignature(link.to());
-				if(s)
-				{
-					// modify neighbor "from"
-					s->changeLinkIds(oldS->id(), newS->id());
-
-					newS->addLink(link);
-				}
-				else
-				{
-					UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
-				}
-			}
-			newS->setLabel(oldS->getLabel());
-			oldS->setLabel("");
-			oldS->removeLinks(); // remove all links
-			oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location
-
-			// Set old image to new signature
-			this->copyData(oldS, newS);
-
-			// update weight
-			newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
-
-			if(_lastGlobalLoopClosureId == oldS->id())
-			{
-				_lastGlobalLoopClosureId = newS->id();
+				UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
+						_rehearsalMaxDistance, _rehearsalMaxAngle);
+				return false;
 			}
+			fullMerge = !isMoving && newS->hasLink(oldS->id());
+			intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
 		}
 		else
 		{
-			newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location
-
-			// update weight
-			oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
-
-			if(_lastSignature == newS)
-			{
-				_lastSignature = oldS;
-			}
+			fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
 		}
 
-		// remove location
-		moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
+		if(fullMerge)
+		{
+			//remove mutual links
+			Link newToOldLink = newS->getLinks().at(oldS->id());
+			oldS->removeLink(newId);
+			newS->removeLink(oldId);
 
-		return true;
+			if(_idUpdatedToNewOneRehearsal)
+			{
+				// redirect neighbor links
+				const std::map<int, Link> & links = oldS->getLinks();
+				for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+				{
+					Link link = iter->second;
+					Link mergedLink = newToOldLink.merge(link, link.type());
+					UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to());
+
+					Signature * s = this->_getSignature(link.to());
+					if(s)
+					{
+						// modify neighbor "from"
+						s->removeLink(oldS->id());
+						s->addLink(mergedLink.inverse());
+
+						newS->addLink(mergedLink);
+					}
+					else
+					{
+						UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id());
+					}
+				}
+				newS->setLabel(oldS->getLabel());
+				oldS->setLabel("");
+				oldS->removeLinks(); // remove all links
+				oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location
+
+				// Set old image to new signature
+				this->copyData(oldS, newS);
+
+				// update weight
+				newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
+
+				if(_lastGlobalLoopClosureId == oldS->id())
+				{
+					_lastGlobalLoopClosureId = newS->id();
+				}
+			}
+			else
+			{
+				newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location
+
+				// update weight
+				oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
+
+				if(_lastSignature == newS)
+				{
+					_lastSignature = oldS;
+				}
+			}
+
+			// remove location
+			moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);
+
+			return true;
+		}
+		else
+		{
+			// update only weights
+			if(_idUpdatedToNewOneRehearsal)
+			{
+				// just update weight
+				int w = oldS->getWeight()>=0?oldS->getWeight():0;
+				newS->setWeight(w + newS->getWeight() + 1);
+				oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+
+				if(_lastGlobalLoopClosureId == oldS->id())
+				{
+					_lastGlobalLoopClosureId = newS->id();
+				}
+			}
+			else // !_idUpdatedToNewOneRehearsal
+			{
+				int w = newS->getWeight()>=0?newS->getWeight():0;
+				oldS->setWeight(w + oldS->getWeight() + 1);
+				newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+			}
+		}
 	}
 	else
 	{
@@ -3273,7 +3287,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
 		}
 	}
-	return false;
+	return false; // means that the newS can be removed without problem
 }
 
 Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
@@ -4599,7 +4613,7 @@ void Memory::getMetricConstraints(
 								const Signature * s2 = this->getSignature(uter->first);
 								if(s2)
 								{
-									link = link.merge(uter->second);
+									link = link.merge(uter->second, uter->second.type());
 									poses.erase(s->id());
 									s = s2;
 								}

corelib/src/Rtabmap.cpp

@@ -409,6 +409,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 
+	UASSERT(_rgbdLinearUpdate >= 0.0f);
+	UASSERT(_rgbdAngularUpdate >= 0.0f);
+
 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
 	{
@@ -986,7 +989,7 @@ bool Rtabmap::process(
 		UFATAL("Not supposed to be here...last signature is null?!?");
 	}
 
-	ULOGGER_INFO("Processing signature %d", signature->id());
+	ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight());
 	timeMemoryUpdate = timer.ticks();
 	ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
 
@@ -1002,7 +1005,7 @@ bool Rtabmap::process(
 		{
 			_optimizedPoses.erase(rehearsedId);
 		}
-		else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
+		else if(signature->getWeight() >= 0 && _rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f)
 		{
 			//============================================================
 			// Minimum displacement required to add to Memory
@@ -1010,20 +1013,25 @@ bool Rtabmap::process(
 			const std::map<int, Link> & links = signature->getLinks();
 			if(links.size() == 1)
 			{
-				float x,y,z, roll,pitch,yaw;
-				links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-				if((_rgbdLinearUpdate==0.0f || (
-					 fabs(x) < _rgbdLinearUpdate &&
-					 fabs(y) < _rgbdLinearUpdate &&
-					 fabs(z) < _rgbdLinearUpdate)) &&
-					(_rgbdAngularUpdate==0.0f || (
-					 fabs(roll) < _rgbdAngularUpdate &&
-					 fabs(pitch) < _rgbdAngularUpdate &&
-					 fabs(yaw) < _rgbdAngularUpdate)))
+				// 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;
+					}
 				}
 			}
 		}
@@ -1088,7 +1096,7 @@ bool Rtabmap::process(
 				UASSERT(s!=0);
 				if(s->getWeight() == -1)
 				{
-					tmp = _constraints.rbegin()->second.merge(tmp);
+					tmp = _constraints.rbegin()->second.merge(tmp, tmp.type());
 					_optimizedPoses.erase(s->id());
 					_constraints.erase(--_constraints.end());
 				}
@@ -1156,7 +1164,7 @@ bool Rtabmap::process(
 	// Bayes filter update
 	//============================================================
 	int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
-	// Not a bad signature, not a small displacemnt unless the previous signature didn't have a loop closure
+	// Not a bad signature, not a small displacement unless the previous signature didn't have a loop closure
 	if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
 	{
 		// If the working memory is empty, don't do the detection. It happens when it

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -205,6 +205,7 @@ public:
 	bool isStatisticsPublished() const;
 	double getLoopThr() const;
 	double getVpThr() const;
+	double getSimThr() const;
 	int getOdomStrategy() const;
 	int getOdomBufferSize() const;
 	QString getCameraInfoDir() const; // "workinfDir/camera_info"

guilib/src/MainWindow.cpp

@@ -1075,23 +1075,28 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			_ui->label_matchId->clear();
 		}
 
-		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
+		int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
+		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
 		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
 		_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
-		if(rehearsed > 0)
+		if(rehearsalMerged > 0)
 		{
 			_ui->imageView_source->setBackgroundColor(Qt::blue);
 		}
 		else if(localTimeClosures > 0)
 		{
-			_ui->imageView_source->setBackgroundColor(Qt::darkCyan);
+			_ui->imageView_source->setBackgroundColor(Qt::darkYellow);
 		}
 		else if(scanMatchingSuccess)
 		{
 			_ui->imageView_source->setBackgroundColor(Qt::gray);
 		}
+		else if(rehearsedSimilarity)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
+		}
 
 		UDEBUG("time= %d ms", time.restart());
 

guilib/src/PreferencesDialog.cpp

@@ -3670,6 +3670,10 @@ double PreferencesDialog::getVpThr() const
 {
 	return _ui->general_doubleSpinBox_vp->value();
 }
+double PreferencesDialog::getSimThr() const
+{
+	return _ui->doubleSpinBox_similarityThreshold->value();
+}
 int PreferencesDialog::getOdomStrategy() const
 {
 	return _ui->odom_strategy->currentIndex();