Updated how local scan matching is done (doing it on segmented local paths)

corelib/include/rtabmap/core/DBReader.h

@@ -49,7 +49,7 @@ public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 float delayToStartSec = 0.0f);
+			 bool ignoreGoalDelay = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -64,7 +64,7 @@ private:
 	std::string _path;
 	float _frameRate;
 	bool _odometryIgnored;
-	float _delayToStartSec;
+	bool _ignoreGoalDelay;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Graph.h

@@ -145,6 +145,10 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,
 		int to);
+std::multimap<int, int>::iterator RTABMAP_EXP findLink(
+		std::multimap<int, int> & links,
+		int from,
+		int to);
 
 /**
  * Get only the the most recent or older poses in the defined radius.

corelib/include/rtabmap/core/Memory.h

@@ -220,6 +220,7 @@ private:
 	bool _generateIds;
 	bool _badSignaturesIgnored;
 	int _imageDecimation;
+	float _laserScanVoxelSize;
 	bool _localSpaceLinksKeptInWM;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
@@ -255,6 +256,8 @@ private:
 	bool _bowForce2D;
 	bool _bowEpipolarGeometry;
 	float _bowEpipolarGeometryVar;
+	float _icpMaxTranslation;
+	float _icpMaxRotation;
 	int _icpDecimation;
 	float _icpMaxDepth;
 	float _icpVoxelSize;

corelib/include/rtabmap/core/Parameters.h

@@ -194,7 +194,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
-	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1).");
+	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, LaserScanVoxelSize,      float, 0.0,      "If > 0.0, voxelize laser scans when creating a signature.");
 	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
 
 
@@ -287,7 +288,8 @@ class RTABMAP_EXP Parameters
 	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).");
-	RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked,  bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
+	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
+	RTABMAP_PARAM(RGBD, PlanVirtualLinksMaxDiffID,  int, 50, "Max difference ID to add virtual links before planning.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
@@ -295,8 +297,8 @@ class RTABMAP_EXP Parameters
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 50,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 50,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 1.0, "Path filtering radius.");
 
 	// Graph optimization
 	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -345,7 +347,8 @@ class RTABMAP_EXP Parameters
 
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
-	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxTranslation,  float, 0.2,      "Maximum ICP translation correction accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -138,7 +138,8 @@ public:
 	int getPathCurrentGoalId() const;
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
-	std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
+	std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -174,13 +175,12 @@ private:
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
 	int _globalLoopClosureIcpType;
-	float _globalLoopClosureIcpMaxDistance;
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localRadius;
-	float _localDetectMaxNeighbors;
 	int _localDetectMaxDiffID;
+	float _localPathFilteringRadius;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	bool _reextractLoopClosureFeatures;
@@ -190,7 +190,8 @@ private:
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
-	bool _planWithNearNodesLinked;
+	bool _planVirtualLinks;
+	int _planVirtualLinksMaxDiffID;
 	bool _goalsSavedInUserData;
 
 	std::pair<int, float> _loopClosureHypothesis;

corelib/include/rtabmap/core/Statistics.h

@@ -53,6 +53,7 @@ namespace rtabmap {
 class RTABMAP_EXP Statistics
 {
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
+	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
@@ -64,9 +65,10 @@ class RTABMAP_EXP Statistics
 
 	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
-	RTABMAP_STATS(LocalLoop, Space_closure_id,);
+	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
-	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
+	RTABMAP_STATS(LocalLoop, Space_paths,);
-	RTABMAP_STATS(LocalLoop, Space_neighbors,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);

corelib/src/DBDriverSqlite3.cpp

@@ -865,9 +865,8 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 		{
 			query << "SELECT id "
 				  << "FROM Node "
-				  << "LEFT OUTER JOIN Link "
+				  << "INNER JOIN Link "
-				  << "ON id = from_id "
+				  << "ON id = to_id " // use to_id tp ignore all children (which don't have link pointing on them)
-				  << "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
 				  << "ORDER BY id";
 		}
 

corelib/src/DBReader.cpp

@@ -45,11 +45,11 @@ namespace rtabmap {
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
-				   float delayToStartSec) :
+				   bool ignoreGoalDelay) :
 	_path(databasePath),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
-	_delayToStartSec(delayToStartSec),
+	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
@@ -126,10 +126,6 @@ void DBReader::setFrameRate(float frameRate)
 
 void DBReader::mainLoopBegin()
 {
-	if(_delayToStartSec > 0.0f)
-	{
-		uSleep(_delayToStartSec*1000.0f);
-	}
 	_timer.start();
 }
 
@@ -174,7 +170,7 @@ void DBReader::mainLoop()
 		{
 			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 
-			if(_currentId != _ids.end())
+			if(!_ignoreGoalDelay && _currentId != _ids.end())
 			{
 				// get stamp for the next signature to compute the delay
 				// that was used originally for planning
@@ -192,10 +188,10 @@ void DBReader::mainLoop()
 							goalId, delay);
 					uSleep(delay*1000);
 				}
-				else
+			}
-				{
+			else
-					UWARN("stamps = %d=%f %d=%f ", data.id(), data.stamp(), *_currentId, stamp);
+			{
-				}
+				UWARN("Goal %d detected, posting it!", goalId);
 			}
 		}
 

corelib/src/Graph.cpp

@@ -907,6 +907,34 @@ std::multimap<int, Link>::iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, int>::iterator findLink(
+		std::multimap<int, int> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, int>::iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
+
 std::map<int, Transform> radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,

corelib/src/Memory.cpp

@@ -69,6 +69,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_generateIds(Parameters::defaultMemGenerateIds()),
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
 	_imageDecimation(Parameters::defaultMemImageDecimation()),
+	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -96,6 +97,9 @@ Memory::Memory(const ParametersMap & parameters) :
 	_bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()),
 	_bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()),
 
+	_icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()),
+	_icpMaxRotation(Parameters::defaultLccIcpMaxRotation()),
+
 	_icpDecimation(Parameters::defaultLccIcp3Decimation()),
 	_icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()),
 	_icpVoxelSize(Parameters::defaultLccIcp3VoxelSize()),
@@ -344,6 +348,7 @@ Memory::~Memory()
 		if(!_memoryChanged && _linksChanged && _dbDriver)
 		{
 			// don't update the time stamps!
+			UDEBUG("");
 			_dbDriver->setTimestampUpdateEnabled(false);
 		}
 		this->clear();
@@ -390,6 +395,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
+	Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -424,6 +430,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
 	Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry);
 	Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
+	Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation);
+	Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation);
 	Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
 	Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth);
 	Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize);
@@ -1033,6 +1041,7 @@ void Memory::clear()
 		UDEBUG("Adding statistics after run...");
 		if(_memoryChanged)
 		{
+			UDEBUG("");
 			_dbDriver->addStatisticsAfterRun(memSize,
 					_lastSignature?_lastSignature->id():0,
 					UProcessInfo::getMemoryUsage(),
@@ -2185,8 +2194,26 @@ Transform Memory::computeIcpTransform(
 					if(!icpT.isNull() && hasConverged &&
 					   correspondencesRatio >= _icpCorrespondenceRatio)
 					{
-						transform = icpT * guess;
+						float x,y,z, roll,pitch,yaw;
-						transform = transform.inverse();
+						icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+						if((_icpMaxTranslation>0.0f &&
+							(x > _icpMaxTranslation ||
+							 y > _icpMaxTranslation ||
+							 z > _icpMaxTranslation))
+							||
+							(_icpMaxRotation>0.0f &&
+							 (roll > _icpMaxRotation ||
+							 pitch > _icpMaxRotation ||
+							 yaw > _icpMaxRotation)))
+						{
+							msg = uFormat("Cannot compute transform (ICP correction too large)");
+							UINFO(msg.c_str());
+						}
+						else
+						{
+							transform = icpT * guess;
+							transform = transform.inverse();
+						}
 					}
 					else
 					{
@@ -2228,7 +2255,7 @@ Transform Memory::computeIcpTransform(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newS.getLaserScanRaw(), guess);
 
 			//voxelize
-			if(_icp2VoxelSize > 0.0f)
+			if(_icp2VoxelSize > _laserScanVoxelSize)
 			{
 				oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, _icp2VoxelSize);
 				newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
@@ -2258,16 +2285,43 @@ Transform Memory::computeIcpTransform(
 						(int)oldCloud->size(),
 						correspondencesRatio*100.0f);
 
+				//pcl::io::savePCDFile("oldCloud.pcd", *oldCloud);
+				//pcl::io::savePCDFile("newCloud.pcd", *newCloud);
+				//UWARN("saved oldCloud.pcd and newCloud.pcd");
+				//if(!icpT.isNull())
+				//{
+				//	newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
+				//	pcl::io::savePCDFile("newCloudFinal.pcd", *newCloud);
+				//	UWARN("saved newCloudFinal.pcd");
+				//}
+
 				if(inliers)
 				{
 					*inliers = correspondences;
 				}
 
-				if(!icpT.isNull() && hasConverged &&
+				if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
-				   correspondencesRatio >= _icp2CorrespondenceRatio)
 				{
-					transform = icpT * guess;
+					float x,y,z, roll,pitch,yaw;
-					transform = transform.inverse();
+					icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+					if((_icpMaxTranslation>0.0f &&
+						(x > _icpMaxTranslation ||
+						 y > _icpMaxTranslation ||
+						 z > _icpMaxTranslation))
+					    ||
+					    (_icpMaxRotation>0.0f &&
+						 (roll > _icpMaxRotation ||
+						 pitch > _icpMaxRotation ||
+						 yaw > _icpMaxRotation)))
+					{
+						msg = uFormat("Cannot compute transform (ICP correction too large)");
+						UINFO(msg.c_str());
+					}
+					else
+					{
+						transform = icpT * guess;
+						transform = transform.inverse();
+					}
 				}
 				else
 				{
@@ -2329,10 +2383,12 @@ Transform Memory::computeScanMatchingTransform(
 	{
 		if(iter->first != newId)
 		{
-			const Signature * s = this->getSignature(iter->first);
+			Signature * s = this->_getSignature(iter->first);
 			if(!s->getLaserScanCompressed().empty())
 			{
-				*assembledOldClouds += *util3d::cvMat2Cloud(rtabmap::uncompressData(s->getLaserScanCompressed()), iter->second);
+				cv::Mat scan;
+				s->uncompressData(0, 0, &scan);
+				*assembledOldClouds += *util3d::cvMat2Cloud(scan, iter->second);
 			}
 			else
 			{
@@ -2348,21 +2404,19 @@ Transform Memory::computeScanMatchingTransform(
 	}
 
 	// get the new cloud
-	const Signature * newS = getSignature(newId);
+	Signature * newS = _getSignature(newId);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
 	UASSERT(uContains(poses, newId));
-	newCloud = util3d::cvMat2Cloud(rtabmap::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
+	cv::Mat newScan;
+	newS->uncompressData(0, 0, &newScan);
+	newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
 
 	//voxelize
-	if(newCloud->size() && _icp2VoxelSize > 0.0f)
+	if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize)
 	{
 		newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
 	}
 
-	//UWARN("local scan matching pcd saved!");
-	//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
-	//pcl::io::savePCDFile("new.pcd", *newCloud);
-
 	Transform transform;
 	if(assembledOldClouds->size() && newCloud->size())
 	{
@@ -2397,8 +2451,11 @@ Transform Memory::computeScanMatchingTransform(
 		{
 			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
 
-			//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(oldId)*transform.inverse());
+			//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
+			//pcl::io::savePCDFile("new.pcd", *newCloud);
+			//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
 			//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
+			//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
 		}
 		else
 		{
@@ -2444,6 +2501,17 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
 
 		UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
 
+		if(rotVariance == 0)
+		{
+			rotVariance = 0.000001; // set small variance (0.001 m x 0.001 m)
+			UWARN("Null rotation variance detected, set to something very small (0.001m^2)!");
+		}
+		if(transVariance == 0)
+		{
+			transVariance = 0.000001; // set small variance (0.001 m x 0.001 m)
+			UWARN("Null transitional variance detected, set to something very small (0.001m^2)!");
+		}
+
 		oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), rotVariance, transVariance));
 		newS->addLink(Link(newS->id(), oldS->id(), type, transform, rotVariance, transVariance));
 
@@ -3330,7 +3398,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	{
 		if(id <= 0)
 		{
-			UWARN("Received image ID is null. "
+			UERROR("Received image ID is null. "
 				  "Please set parameter Mem/GenerateIds to \"true\" or "
 				  "make sure the input source provides image ids (seq).");
 			return 0;
@@ -3341,7 +3409,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 		}
 		else
 		{
-			UWARN("Id of acquired image (%d) is smaller than the last in memory (%d). "
+			UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
 				  "Please set parameter Mem/GenerateIds to \"true\" or "
 				  "make sure the input source provides image ids (seq) over the last in "
 				  "memory, which is %d.",
@@ -3758,6 +3826,13 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 		}
 	}
 
+	// apply icp2 voxel?
+	cv::Mat laserScan = data.laserScan();
+	if(!laserScan.empty() && _laserScanVoxelSize > 0.0f)
+	{
+		laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize<pcl::PointXYZ>(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize));
+	}
+
 	Signature * s;
 	if(this->isBinDataKept())
 	{
@@ -3772,7 +3847,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 
 		rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
 		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
-		rtabmap::CompressionThread ctDepth2d(data.laserScan());
+		rtabmap::CompressionThread ctDepth2d(laserScan);
 		ctImage.start();
 		ctDepth.start();
 		ctDepth2d.start();
@@ -3809,13 +3884,13 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			words3D,
 			data.pose(),
 			data.userData(),
-			rtabmap::compressData2(data.laserScan()));
+			rtabmap::compressData2(laserScan));
 	}
 	if(this->isRawDataKept())
 	{
 		s->setImageRaw(image);
 		s->setDepthRaw(depthOrRightImage);
-		s->setLaserScanRaw(data.laserScan());
+		s->setLaserScanRaw(laserScan);
 	}
 
 

corelib/src/Rtabmap.cpp

@@ -92,13 +92,12 @@ Rtabmap::Rtabmap() :
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
 	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
 	_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
-	_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
 	_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
-	_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
 	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
+	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -108,7 +107,8 @@ Rtabmap::Rtabmap() :
 	_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
-	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
+	_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
+	_planVirtualLinksMaxDiffID(Parameters::defaultRGBDPlanVirtualLinksMaxDiffID()),
 	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
@@ -381,12 +381,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
 	Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
-	Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
-	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -395,7 +394,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
-	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
+	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
+	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinksMaxDiffID(), _planVirtualLinksMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 
 	// RGB-D SLAM stuff
@@ -996,20 +996,8 @@ bool Rtabmap::process(const SensorData & data)
 					Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
 					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 					{
-						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
+						transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
-						float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
+						variance = 1.0f; // ICP, set variance to 1
-						if(!icpTransform.isNull() &&
-							_globalLoopClosureIcpMaxDistance>0.0f &&
-							squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
-						{
-							UWARN("Local loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
-									signature->id(),
-									*iter,
-									squaredNorm,
-									_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
-							icpTransform.setNull();
-						}
-						transform = icpTransform;
 					}
 					if(!transform.isNull())
 					{
@@ -1361,14 +1349,17 @@ bool Rtabmap::process(const SensorData & data)
 			{
 				nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
 			}
-			for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
+			for(std::map<float, int>::iterator iter=nearNodesByDist.begin();
 				iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
 				++iter)
 			{
 				const Signature * s = _memory->getSignature(iter->second);
-				UASSERT(s != 0);
+				UASSERT(s!=0);
-				for(std::map<int, Link>::const_iterator jter=s->getLinks().begin();
+				// If there is a change of direction, better to be retrieving
-					jter!=s->getLinks().end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+				// ALL nearest signatures than only newest neighbors
+				const std::map<int, Link> & links = s->getLinks();
+				for(std::map<int, Link>::const_iterator jter=links.begin();
+					jter!=links.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
 					++jter)
 				{
 					if(_memory->getSignature(jter->first) == 0)
@@ -1488,20 +1479,8 @@ bool Rtabmap::process(const SensorData & data)
 			}
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				Transform icpTransform  = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
+				transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
-				float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
+				variance = 1.0f; // ICP, set variance to 1
-				if(!icpTransform.isNull() &&
-					_globalLoopClosureIcpMaxDistance>0.0f &&
-					squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
-				{
-					UWARN("Global loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
-							signature->id(),
-							_loopClosureHypothesis.first,
-							squaredNorm,
-							_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
-					icpTransform.setNull();
-				}
-				transform = icpTransform;
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1532,11 +1511,11 @@ bool Rtabmap::process(const SensorData & data)
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
 
-	int localSpaceDetectionPosesCount = 0;
+	int localSpaceClosuresAdded = 0;
-	int localSpaceClosureId = 0;
+	int localSpaceClosuresAddedByICPOnly = 0;
-	int localSpaceNearestId = 0;
+	int lastLocalSpaceClosureId = 0;
-	if(_loopClosureHypothesis.first == 0 &&
+	int localSpacePaths = 0;
-	   _localLoopClosureDetectionSpace &&
+	if(_localLoopClosureDetectionSpace &&
 	   !signature->getLaserScanCompressed().empty())
 	{
 		if(_graphOptimizer->iterations() == 0)
@@ -1548,43 +1527,93 @@ bool Rtabmap::process(const SensorData & data)
 			//============================================================
 			// Scan matching LOCAL LOOP CLOSURE SPACE
 			//============================================================
-			std::map<int, Transform> localSpacePoses;
+			std::map<int, Transform> forwardPoses;
-			localSpaceNearestId = 0;
+			forwardPoses = this->getForwardWMPoses(
-			localSpacePoses = this->getWMPosesInRadius(
 					signature->id(),
-					_localDetectMaxNeighbors,
+					0,
 					_localRadius,
-					_localDetectMaxDiffID,
+					_localDetectMaxDiffID);
-					localSpaceNearestId);
 
-			// add current node to poses
+			std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
-			localSpacePoses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+			localSpacePaths = forwardPaths.size();
-			localSpaceDetectionPosesCount = (int)localSpacePoses.size()-1;
+
-			//The nearest will be the reference for a loop closure transform
+			for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
-			if(localSpacePoses.size() &&
-				localSpaceNearestId &&
-				signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
 			{
-				double variance = 1.0;
+				std::map<int, Transform> & path = *iter;
-				std::string rejectedMsg;
+				UASSERT(path.size());
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, localSpacePoses, &rejectedMsg, 0, &variance);
-				if(!t.isNull())
-				{
-					localSpaceClosureId = localSpaceNearestId;
-					UINFO("Add local loop closure in SPACE (%d->%d) %s",
-							signature->id(),
-							localSpaceNearestId,
-							t.prettyPrint().c_str());
-					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, 1, 1); // set Identify covariance
 
-					// Old map -> new map, used for localization correction on loop closure
+				// only do local loop closure detection if there is no
-					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
+				// global loop closure already detected on this path
-					UASSERT(oldS != 0);
+				if(_loopClosureHypothesis.first == 0 || path.find(_loopClosureHypothesis.first) == path.end())
-					_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
-				}
-				else
 				{
-					UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
+					//find the nearest pose on the path
+					int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
+					UASSERT(nearestId > 0);
+
+					// nearest pose must be close
+					if(_localPathFilteringRadius <= 0.0f ||
+					   _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
+					{
+						// path filtering
+						if(_localPathFilteringRadius > 0.0f)
+						{
+							path = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
+							path.insert(*_optimizedPoses.find(nearestId)); // make sure the nearest pose is still here
+						}
+
+						// 1) look for loop closures based on visual correspondences
+						double variance = 1.0;
+						Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
+						bool foundByVisual = false;
+						if(!transform.isNull() && _globalLoopClosureIcpType > 0)
+						{
+							transform  = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
+							variance = 1.0f; // ICP, set variance to 1
+						}
+						if(transform.isNull())
+						{
+							if(path.size() > 2) // more than current+nearest
+							{
+								// 2) Assemble scans in the path and do ICP only
+								// add current node to poses
+								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+								//The nearest will be the reference for a loop closure transform
+								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
+								{
+									transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
+								}
+							}
+						}
+						else
+						{
+							foundByVisual = true;
+						}
+
+						if(!transform.isNull())
+						{
+							UINFO("Add local loop closure in SPACE (%d->%d) %s",
+									signature->id(),
+									nearestId,
+									transform.prettyPrint().c_str());
+							// set Identify covariance if laser scan matching only
+							_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, foundByVisual?variance:1, foundByVisual?variance:1);
+
+							// Old map -> new map, used for localization correction on loop closure
+							const Signature * oldS = _memory->getSignature(nearestId);
+							UASSERT(oldS != 0);
+							_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
+							++localSpaceClosuresAdded;
+							if(!foundByVisual)
+							{
+								++localSpaceClosuresAddedByICPOnly;
+							}
+							lastLocalSpaceClosureId = nearestId;
+						}
+						else
+						{
+							UINFO("Local loop closure %d (space) rejected", nearestId);
+						}
+					}
 				}
 			}
 		}
@@ -1599,7 +1628,7 @@ bool Rtabmap::process(const SensorData & data)
 		(_loopClosureHypothesis.first>0 ||				// can be different map of the current one
 		 localLoopClosuresInTimeFound>0 || 	// only same map of the current one
 		 scanMatchingSuccess || 			// only same map of the current one
-		 localSpaceClosureId>0 || 			// can be different map of the current one
+		 lastLocalSpaceClosureId>0 || 	    // can be different map of the current one
 		 signaturesRetrieved.size()))  		// can be different map of the current one
 	{
 		if(_memory->isIncremental())
@@ -1616,10 +1645,10 @@ bool Rtabmap::process(const SensorData & data)
 				UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
 			}
 		}
-		else if(_loopClosureHypothesis.first > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
+		else if(_loopClosureHypothesis.first > 0 || lastLocalSpaceClosureId > 0 || signaturesRetrieved.size())
 		{
 			UINFO("Update map correction: Localization mode");
-			int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first;
+			int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first;
 			UASSERT(oldId != 0);
 			if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
 			{
@@ -1672,7 +1701,7 @@ bool Rtabmap::process(const SensorData & data)
 	int lcHypothesisReactivated = 0;
 	float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
 	int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
-	sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first);
+	sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first);
 	if(sLoop)
 	{
 		lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
@@ -1699,6 +1728,7 @@ bool Rtabmap::process(const SensorData & data)
 			ULOGGER_INFO("send all stats...");
 			statistics_.setExtended(1);
 
+			statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
 			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
 			statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
@@ -1710,11 +1740,12 @@ bool Rtabmap::process(const SensorData & data)
 
 			statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
-			statistics_.setLocalLoopClosureId(localSpaceClosureId);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
-			if(_loopClosureHypothesis.first || localSpaceClosureId)
+			statistics_.setLocalLoopClosureId(lastLocalSpaceClosureId);
+			if(_loopClosureHypothesis.first || lastLocalSpaceClosureId)
 			{
 				UASSERT(uContains(sLoop->getLinks(), signature->id()));
 				UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
@@ -2099,13 +2130,13 @@ void Rtabmap::dumpData() const
 }
 
 // fromId must be in _memory and in _optimizedPoses
-// Get poses in front of the robot
+// Get poses in front of the robot, return optimized poses
-std::map<int, Transform> Rtabmap::getWMPosesInRadius(
+std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		int fromId,
 		int maxNearestNeighbors,
 		float radius,
-		int maxDiffID, // 0 means ignore
+		int maxDiffID // 0 means ignore
-		int & nearestId) const
+		) const
 {
 	std::map<int, Transform> poses;
 	if(_memory && fromId > 0)
@@ -2127,12 +2158,15 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		}
 		for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
-			// Only locations in Working Memory not too far from the current node (so inside the margin)
+			if(iter->first != fromId)
-			bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
-			if(stm.find(iter->first) == stm.end() && diffIdOk)
 			{
-				(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+				// Only locations in Working Memory not too far from the current node (so inside the margin)
-				ids[oi++] = iter->first;
+				bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
+				if(stm.find(iter->first) == stm.end() && diffIdOk)
+				{
+					(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+					ids[oi++] = iter->first;
+				}
 			}
 		}
 
@@ -2142,8 +2176,6 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
 		Transform fromT = _optimizedPoses.at(fromId);
 
-		nearestId = 0;
-		float minDistance = -1;
 		if(cloud->size())
 		{
 			//if(cloud->size())
@@ -2153,17 +2185,16 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 			//}
 
 			//filter poses in front of the fromId
-			Transform t=Transform::getIdentity();
-			t.x() = radius*0.95f;
 			float x,y,z, roll,pitch,yaw;
-			(fromT*t).getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 
 			pcl::CropBox<pcl::PointXYZ> cropbox;
 			cropbox.setInputCloud(cloud);
-			cropbox.setMin(Eigen::Vector4f(-radius, -radius, -radius, 0));
+			cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
-			cropbox.setMax(Eigen::Vector4f(radius, radius, radius, 0));
+			cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
 			cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
 			cropbox.setTranslation(Eigen::Vector3f(x, y, z));
+			cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
 			pcl::IndicesPtr indices(new std::vector<int>());
 			cropbox.filter(*indices);
 
@@ -2190,11 +2221,6 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 						//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
 						UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
 						poses.insert(std::make_pair(ids[ind[i]], tmp));
-						if(minDistance == -1 || minDistance > dist[i])
-						{
-							nearestId = ids[ind[i]];
-							minDistance = dist[i];
-						}
 					}
 				}
 
@@ -2209,19 +2235,42 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 				//	c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
 				//	pcl::io::savePCDFile("radiusNearestPt.pcd", c);
 				//}
-
-				if(nearestId == 0 && poses.size())
-				{
-					UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId);
-					poses.clear();
-				}
 			}
 		}
-		UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
 	}
 	return poses;
 }
 
+// Get paths in front of the robot, returned optimized poses
+std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
+{
+	std::list<std::map<int, Transform> > paths;
+	if(_memory && poses.size())
+	{
+		// Segment poses connected only by neighbor links
+		while(poses.size())
+		{
+			std::map<int, Transform> path;
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
+			{
+				if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first))
+				{
+					path.insert(*iter);
+					poses.erase(iter++);
+				}
+				else
+				{
+					break;
+				}
+			}
+			UASSERT(path.size());
+			paths.push_back(path);
+		}
+
+	}
+	return paths;
+}
+
 void Rtabmap::optimizeCurrentMap(
 		int id,
 		bool lookInDatabase,
@@ -2556,10 +2605,20 @@ bool Rtabmap::computePath(
 			links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
 		}
 		// Add links between neighbor nodes in the goal radius.
-		if(_planWithNearNodesLinked)
+		if(_planVirtualLinks)
 		{
 			std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
-			links.insert(clusters.begin(), clusters.end());
+			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
+			{
+				if(graph::findLink(links, iter->first, iter->second) != links.end())
+				{
+					if(_planVirtualLinksMaxDiffID <= 0 ||
+					   abs(iter->first - iter->second) < _planVirtualLinksMaxDiffID)
+					{
+						links.insert(*iter);
+					}
+				}
+			}
 		}
 
 		UINFO("Computing path from location %d to %d", currentNode, targetNode);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -176,6 +176,7 @@ public:
 	int getSourceUsbDeviceId() const;		//UsbDevice group
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
+	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	Src getSourceRGBD() const; 			// Openni group
 	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group

guilib/src/MainWindow.cpp

@@ -602,7 +602,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
 		Statistics stats = rtabmapEvent->getStats();
 		int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
-		int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_closure_id(), 0.0f));
+		int localLoopClosureId = int(uValue(stats.data(), Statistics::kLocalLoopSpace_last_closure_id(), 0.0f));
 		bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
 		float highestHypothesisValue = uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
 		if((stats.loopClosureId() > 0 &&
@@ -1984,6 +1984,9 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
 	else
 	{
 		_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
+		_ui->statusbar->showMessage(
+				tr("Global path computed from %1 (%2 poses, %3 m)!").arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
+				5000);
 	}
 }
 
@@ -2730,7 +2733,8 @@ void MainWindow::startDetection()
 	{
 		_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
 								 _preferencesDialog->getGeneralInputRate(),
-								 _preferencesDialog->getSourceDatabaseOdometryIgnored());
+								 _preferencesDialog->getSourceDatabaseOdometryIgnored(),
+								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored());
 
 		//Create odometry thread if rgdb slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&

guilib/src/PreferencesDialog.cpp

@@ -293,6 +293,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->groupBox_sourceDatabase, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_database_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//openni group
 	connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
@@ -363,6 +364,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
 	_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
 	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
+	_ui->general_doubleSpinBox_laserScanVoxel->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str());
 
 
 	// Database
@@ -475,14 +477,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
-	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
+	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
+	_ui->graphPlan_maxDiffID->setObjectName(Parameters::kRGBDPlanVirtualLinksMaxDiffID().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
-	_ui->localDetection_maxNeighbors->setObjectName(Parameters::kRGBDLocalLoopDetectionNeighbors().c_str());
 	_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
+	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
 
 	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
@@ -499,7 +502,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->reextract_maxFeatures->setObjectName(Parameters::kLccReextractMaxWords().c_str());
 
 	_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
-	_ui->globalDetection_icpMaxDistance->setObjectName(Parameters::kLccIcpMaxDistance().c_str());
+	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kLccIcpMaxTranslation().c_str());
+	_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kLccIcpMaxRotation().c_str());
 
 	_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
 	_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());
@@ -727,9 +731,12 @@ void PreferencesDialog::setupSignals()
 	const rtabmap::ParametersMap & parameters = Parameters::getDefaultParameters();
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		QObject * obj = _ui->stackedWidget->findChild<QObject*>((*iter).first.c_str());
+		QWidget * obj = _ui->stackedWidget->findChild<QWidget*>((*iter).first.c_str());
 		if(obj)
 		{
+			// set tooltip as the parameter name
+			obj->setToolTip(iter->first.c_str());
+
 			QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
 			QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
 			QComboBox * combo = qobject_cast<QComboBox *>(obj);
@@ -767,12 +774,12 @@ void PreferencesDialog::setupSignals()
 			}
 			else
 			{
-				ULOGGER_WARN("QObject called %s can't be cast to a supported widget", (*iter).first.c_str());
+				ULOGGER_WARN("QWidget called %s can't be cast to a supported widget", (*iter).first.c_str());
 			}
 		}
 		else
 		{
-			ULOGGER_WARN("Can't find the related QObject for parameter %s", (*iter).first.c_str());
+			ULOGGER_WARN("Can't find the related QWidget for parameter %s", (*iter).first.c_str());
 		}
 	}
 }
@@ -928,6 +935,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->groupBox_sourceDatabase->setChecked(false);
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
+		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 
 		_ui->groupBox_sourceOpenni->setChecked(true);
@@ -1202,6 +1210,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->groupBox_sourceDatabase->setChecked(settings.value("databaseUsed", _ui->groupBox_sourceDatabase->isChecked()).toBool());
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
+	_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
 	_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
 	settings.endGroup(); // Database
 
@@ -1468,6 +1477,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("databaseUsed", 	_ui->groupBox_sourceDatabase->isChecked());
 	settings.setValue("path", 			_ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked());
+	settings.setValue("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked());
 	settings.setValue("startPos",       _ui->source_spinBox_databaseStartPos->value());
 	settings.endGroup();
 
@@ -2030,6 +2040,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
 
 		_ui->groupBox_sourceDatabase->setChecked(true);
 		_ui->source_checkBox_ignoreOdometry->setChecked(r != QMessageBox::Yes);
+		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
 		_ui->source_database_lineEdit_path->setText(path);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 	}
@@ -3036,6 +3047,10 @@ bool PreferencesDialog::getSourceDatabaseOdometryIgnored() const
 {
 	return _ui->source_checkBox_ignoreOdometry->isChecked();
 }
+bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const
+{
+	return _ui->source_checkBox_ignoreGoalDelay->isChecked();
+}
 int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>759</width>
+            <width>760</width>
-            <height>800</height>
+            <height>1002</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>26</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1603,6 +1603,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <bool>false</bool>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_9">
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="0">
                         <widget class="QToolButton" name="source_database_toolButton_selectSource">
                          <property name="text">
@@ -1613,14 +1620,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item row="0" column="1">
                         <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                        </item>
-                       <item row="2" column="1">
+                       <item row="3" column="1">
                         <widget class="QLabel" name="label_58">
                          <property name="text">
                           <string>Start position (index)</string>
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="0">
+                       <item row="3" column="0">
                         <widget class="QSpinBox" name="source_spinBox_databaseStartPos">
                          <property name="minimum">
                           <number>0</number>
@@ -1644,13 +1651,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
-                        <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="1">
                         <widget class="QLabel" name="label_72">
                          <property name="text">
@@ -1661,6 +1661,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_80">
+                         <property name="text">
+                          <string>Ignore goal delay.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -2731,6 +2748,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_recentWmRatio">
+                        <property name="minimumSize">
+                         <size>
+                          <width>50</width>
+                          <height>0</height>
+                         </size>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.200000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -2764,25 +2800,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_recentWmRatio">
-                        <property name="minimumSize">
-                         <size>
-                          <width>50</width>
-                          <height>0</height>
-                         </size>
-                        </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.200000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="1" column="1">
                        <widget class="QLabel" name="label_ratioRecent">
                         <property name="text">
@@ -2916,6 +2933,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_retrieved_8">
+                        <property name="text">
+                         <string>If &gt; 0.0, voxelize laser scans when creating a location. This feature can be used to save laser scans already voxelized.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxel">
+                        <property name="minimumSize">
+                         <size>
+                          <width>50</width>
+                          <height>0</height>
+                         </size>
+                        </property>
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -5157,6 +5209,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item>
                         <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
                          <item row="1" column="0">
+                          <widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>1</number>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>1.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="0">
                           <widget class="QSpinBox" name="localDetection_maxDiffID">
                            <property name="maximum">
                             <number>999</number>
@@ -5166,14 +5234,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="0" column="0">
+                         <item row="0" column="1">
-                          <widget class="QSpinBox" name="localDetection_maxNeighbors">
-                           <property name="value">
-                            <number>10</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
                           <widget class="QLabel" name="label_space3_2">
                            <property name="text">
                             <string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
@@ -5183,16 +5244,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_space3">
-                           <property name="text">
-                            <string>Maximum nearest neighbors for space detection.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
                            <property name="text">
@@ -5210,6 +5261,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_space3_3">
+                           <property name="text">
+                            <string>Path filtering radius to avoid merging laser scans which are close.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5263,14 +5324,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_space3_4">
                            <property name="text">
-                            <string>Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by &quot;Goal reached radius&quot; above.</string>
+                            <string>Add virtual links. Before planning in the graph, near nodes are linked together. The maximum distance is defined by &quot;Goal reached radius&quot; above. If &quot;Maximum ID difference&quot; below is set, only close nodes in time can be linked together.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QLabel" name="label_space3_5">
                            <property name="text">
                             <string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -5280,13 +5341,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_space3_6">
+                           <property name="text">
+                            <string>Maximum ID difference between close nodes to add virtual links. Set 0 to ignore.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QSpinBox" name="graphPlan_maxDiffID">
+                           <property name="maximum">
+                            <number>999</number>
+                           </property>
+                           <property name="value">
+                            <number>50</number>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5754,7 +5835,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                     <item>
                      <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
                       <item row="0" column="0">
-                       <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
+                       <widget class="QDoubleSpinBox" name="globalDetection_icpMaxTranslation">
                         <property name="suffix">
                          <string> m</string>
                         </property>
@@ -5775,13 +5856,42 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_51">
                         <property name="text">
-                         <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
+                         <string>Maximum ICP translation correction accepted (&gt;0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
                         </property>
                        </widget>
                       </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_55">
+                        <property name="text">
+                         <string>Maximum ICP rotation correction accepted (&gt;0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="globalDetection_icpMaxRotation">
+                        <property name="suffix">
+                         <string> rad</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>3.140000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.780000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

tools/OdometryViewer/main.cpp

@@ -705,10 +705,16 @@ int main (int argc, char * argv[])
 
 	if(inputDatabase.size())
 	{
-		rtabmap::DBReader camera(inputDatabase, rate, true, sec);
+		rtabmap::DBReader camera(inputDatabase, rate, true);
 		if(camera.init())
 		{
 			odomThread.start();
+
+			if(sec > 0)
+			{
+				uSleep(sec*1000);
+			}
+
 			camera.start();
 
 			app.exec();