Local space selection: compare maxDiff with margins to current node instead of ids

corelib/include/rtabmap/core/Rtabmap.h

@@ -186,8 +186,6 @@ private:
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
-	int _lastLocalLoopClosureParentId;
-	int _lastLocalLoopClosureChildId;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.

corelib/include/rtabmap/core/Statistics.h

@@ -67,9 +67,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(LocalLoop, Space_closure_id,);
 	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
 	RTABMAP_STATS(LocalLoop, Space_neighbors,);
-	RTABMAP_STATS(LocalLoop, Space_diff_id,);
-	RTABMAP_STATS(LocalLoop, Space_last_parent,);
-	RTABMAP_STATS(LocalLoop, Space_last_child,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);

corelib/src/Rtabmap.cpp

@@ -112,8 +112,6 @@ Rtabmap::Rtabmap() :
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
-	_lastLocalLoopClosureParentId(0),
-	_lastLocalLoopClosureChildId(0),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
 	_memory(0),
@@ -595,8 +593,6 @@ int Rtabmap::triggerNewMap()
 		UINFO("New map triggered, new map = %d", mapId);
 		_optimizedPoses.clear();
 		_constraints.clear();
-		_lastLocalLoopClosureParentId = 0;
-		_lastLocalLoopClosureChildId = 0;
 	}
 	return mapId;
 }
@@ -659,8 +655,6 @@ void Rtabmap::resetMemory()
 	_highestHypothesis = std::make_pair(0,0.0f);
 	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
-	_lastLocalLoopClosureParentId = 0;
-	_lastLocalLoopClosureChildId = 0;
 	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
@@ -1378,11 +1372,6 @@ bool Rtabmap::process(const SensorData & data)
 		{
 			// Make the new one the parent of the old one
 			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
-			if(!rejectedHypothesis)
-			{
-				_lastLocalLoopClosureParentId = signature->id();
-				_lastLocalLoopClosureChildId = _loopClosureHypothesis.first;
-			}
 		}
 
 		if(rejectedHypothesis)
@@ -1455,9 +1444,6 @@ bool Rtabmap::process(const SensorData & data)
 							t.prettyPrint().c_str());
 					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
 
-					_lastLocalLoopClosureParentId = signature->id();
-					_lastLocalLoopClosureChildId = localSpaceNearestId;
-
 					// Old map -> new map, used for localization correction on loop closure
 					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
 					UASSERT(oldS != 0);
@@ -1577,18 +1563,6 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
 			statistics_.setLocalLoopClosureId(localSpaceClosureId);
-			if(localSpaceNearestId)
-			{
-				int d1 = abs(signature->id() - localSpaceNearestId);
-				int d2 = abs(localSpaceNearestId - _lastLocalLoopClosureChildId);
-				int d3 = abs(signature->id() - _lastLocalLoopClosureParentId);
-				int d = d1<=d2?d1:d2;
-				d = d <= d3?d:d3;
-				statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
-			}
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_parent(), _lastLocalLoopClosureParentId);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_child(), _lastLocalLoopClosureChildId);
-
 			if(_loopClosureHypothesis.first || localSpaceClosureId)
 			{
 				UASSERT(uContains(sLoop->getLinks(), signature->id()));
@@ -1929,13 +1903,16 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 	std::vector<int> ids(_optimizedPoses.size());
 	int oi = 0;
 	const std::set<int> & stm = _memory->getStMem();
+	//get margins
+	std::map<int, int> margins;
+	if(maxDiffID > 0)
+	{
+		margins = _memory->getNeighborsId(fromId, maxDiffID, 0, true, false);
+	}
 	for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 	{
-		// Only locations in Working Memory with ID not too far from the last loop closure child id
+		// Only locations in Working Memory not too far from the current node (so inside the margin)
-		bool diffIdOk = maxDiffID == 0 ||
+		bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
-				abs(fromId - iter->first) <= maxDiffID ||
-				(abs(iter->first - _lastLocalLoopClosureChildId) <= maxDiffID &&
-				 abs(fromId - _lastLocalLoopClosureParentId) <= maxDiffID);
 		if(stm.find(iter->first) == stm.end() && diffIdOk)
 		{
 			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());