Increased database version to 0.8.11 (new Depth.data2d_max_pts column). Updated how local loop closure detection in space is done. Update GraphViewer with local radius ellipse and current goal node color. MainWindow saving/loading figures automatically accordingly to the previous session saved.

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 8)
-SET(RTABMAP_PATCH_VERSION 11)
+SET(RTABMAP_PATCH_VERSION 12)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/DBReader.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 
 #include <set>
+#include <list>
 
 namespace rtabmap {
 
@@ -50,6 +51,10 @@ public:
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false);
+	DBReader(const std::list<std::string> & databasePaths,
+			 float frameRate = 0.0f,
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -61,7 +66,7 @@ protected:
 	virtual void mainLoop();
 
 private:
-	std::string _path;
+	std::list<std::string> _paths;
 	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;

corelib/include/rtabmap/core/Graph.h

@@ -207,7 +207,10 @@ int RTABMAP_EXP findNearestNode(
 std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
-		int maxNearestNeighbors,
+		float radius);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
 		float radius);
 
 float RTABMAP_EXP computePathLength(

corelib/include/rtabmap/core/Memory.h

@@ -84,12 +84,18 @@ public:
 	bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
 	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
 	void removeAllVirtualLinks();
-	std::map<int, int> getNeighborsId(int signatureId,
-			int margin,
+	std::map<int, int> getNeighborsId(
+			int signatureId,
+			int maxGraphDepth,
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
+	std::map<int, float> getNeighborsIdRadius(
+			int signatureId,
+			float radius,
+			const std::map<int, Transform> & optimizedPoses,
+			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
 	void removeLink(int idA, int idB);
 
@@ -224,6 +230,7 @@ private:
 	bool _localSpaceLinksKeptInWM;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
+	bool _rehearsalWeightIgnoredWhileMoving;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Parameters.h

@@ -191,6 +191,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
 	RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false,  "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
+	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, true, "When the robot is moving, weights are not updated on rehearsal.");
 	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.");
@@ -297,8 +298,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, 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, 0.25, "Path filtering radius.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 20,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
 
 	// Graph optimization

corelib/include/rtabmap/core/Rtabmap.h

@@ -184,7 +184,7 @@ private:
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localRadius;
-	int _localDetectMaxDiffID;
+	int _localDetectMaxGraphDepth;
 	float _localPathFilteringRadius;
 	bool _localPathOdomPosesUsed;
 	std::string _databasePath;

corelib/include/rtabmap/core/Signature.h

@@ -129,7 +129,7 @@ public:
 	//metric stuff
 	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
 	void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
-	void setLaserScanCompressed(const cv::Mat & bytes, int maxPts = 0) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
+	void setLaserScanCompressed(const cv::Mat & bytes, int maxPts) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
 	void setLocalTransform(const Transform & t) {_localTransform = t;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
@@ -147,7 +147,7 @@ public:
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
 	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts = 0) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
+	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
 	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
 	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 

corelib/include/rtabmap/core/Statistics.h

@@ -66,7 +66,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
 	RTABMAP_STATS(LocalLoop, Space_paths,);
-	RTABMAP_STATS(LocalLoop, Space_closures_added,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_visually,);
 	RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
 
 	RTABMAP_STATS(OdomCorrection, Accepted,);
@@ -81,6 +81,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Local_graph_size,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);
@@ -147,6 +148,7 @@ public:
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
 	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
+	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -169,6 +171,7 @@ public:
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
 	const std::vector<int> & localPath() const {return _localPath;}
+	int currentGoalId() const {return _currentGoalId;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -199,6 +202,7 @@ private:
 	std::map<int, float> _rawLikelihood;
 
 	std::vector<int> _localPath;
+	int _currentGoalId;
 
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}

corelib/src/DBReader.cpp

@@ -46,14 +46,26 @@ DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay) :
-	_path(databasePath),
+	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
+}
 
+DBReader::DBReader(const std::list<std::string> & databasePaths,
+				   float frameRate,
+				   bool odometryIgnored,
+				   bool ignoreGoalDelay) :
+   _paths(databasePaths),
+   _frameRate(frameRate),
+	_odometryIgnored(odometryIgnored),
+	_ignoreGoalDelay(ignoreGoalDelay),
+	_dbDriver(0),
+	_currentId(_ids.end())
+{
 }
 
 DBReader::~DBReader()
@@ -77,9 +89,16 @@ bool DBReader::init(int startIndex)
 	_currentId=_ids.end();
 	_previousStamp = 0;
 
-	if(!UFile::exists(_path))
+	if(_paths.size() == 0)
 	{
-		UERROR("Database path does not exist (%s)", _path.c_str());
+		UERROR("No database path set...");
+		return false;
+	}
+
+	std::string path = _paths.front();
+	if(!UFile::exists(path))
+	{
+		UERROR("Database path does not exist (%s)", path.c_str());
 		return false;
 	}
 
@@ -91,9 +110,9 @@ bool DBReader::init(int startIndex)
 		UERROR("Driver doesn't exist.");
 		return false;
 	}
-	if(!_dbDriver->openConnection(_path))
+	if(!_dbDriver->openConnection(path))
 	{
-		UERROR("Can't open database %s", _path.c_str());
+		UERROR("Can't open database %s", path.c_str());
 		delete _dbDriver;
 		_dbDriver = 0;
 		return false;
@@ -197,8 +216,23 @@ void DBReader::mainLoop()
 	else if(!this->isKilled())
 	{
 		UINFO("no more images...");
-		this->kill();
-		this->post(new CameraEvent());
+		if(_paths.size() > 1)
+		{
+			_paths.pop_front();
+			UWARN("Loading next database \"%s\"...", _paths.front().c_str());
+			if(!this->init())
+			{
+				UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
+				this->kill();
+				this->post(new CameraEvent());
+			}
+		}
+		else
+		{
+			this->kill();
+			this->post(new CameraEvent());
+		}
+
 	}
 
 }

corelib/src/Graph.cpp

@@ -1122,7 +1122,7 @@ public:
 	rtabmap::Transform pose() const {return pose_;}
 	float distFrom(const rtabmap::Transform & pose) const
 	{
-		return pose_.getDistanceSquared(pose); // use sqrt distance
+		return pose_.getDistance(pose); // use sqrt distance
 	}
 
 	void setClosed(bool closed) {closed_ = closed;}
@@ -1287,7 +1287,6 @@ int findNearestNode(
 std::map<int, float> getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
-		int maxNearestNeighbors,
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
@@ -1323,7 +1322,7 @@ std::map<int, float> getNodesInRadius(
 		std::vector<int> ind;
 		std::vector<float> dist;
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
-		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
+		kdTree->radiusSearch(pt, radius, ind, dist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
@@ -1337,6 +1336,59 @@ std::map<int, float> getNodesInRadius(
 	return foundNodes;
 }
 
+// return <id, Transform>, excluding query
+std::map<int, Transform> getPosesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		float radius)
+{
+	UASSERT(uContains(nodes, nodeId));
+	std::map<int, Transform> foundNodes;
+	if(nodes.size() <= 1)
+	{
+		return foundNodes;
+	}
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(nodes.size());
+	std::vector<int> ids(nodes.size());
+	int oi = 0;
+	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	{
+		if(iter->first != nodeId)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+			ids[oi] = iter->first;
+			++oi;
+		}
+	}
+	cloud->resize(oi);
+	ids.resize(oi);
+
+	Transform fromT = nodes.at(nodeId);
+
+	if(cloud->size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		kdTree->radiusSearch(pt, radius, ind, dist, 0);
+		for(unsigned int i=0; i<ind.size(); ++i)
+		{
+			if(ind[i] >=0)
+			{
+				UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
+				foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+			}
+		}
+	}
+	UDEBUG("found nodes=%d", (int)foundNodes.size());
+	return foundNodes;
+}
+
 float computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex,

corelib/src/Memory.cpp

@@ -73,6 +73,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
+	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -399,6 +400,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
+	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -837,14 +839,14 @@ std::map<int, Link> Memory::getLoopClosureLinks(
 // maxCheckedInDatabase = -1 means no limit to check in database (default)
 // maxCheckedInDatabase = 0 means don't check in database
 std::map<int, int> Memory::getNeighborsId(int signatureId,
-		int margin, // 0 means infinite margin
+		int maxGraphDepth, // 0 means infinite margin
 		int maxCheckedInDatabase, // default -1 (no limit)
 		bool incrementMarginOnLoop, // default false
 		bool ignoreLoopIds, // default false
 		double * dbAccessTime
 		) const
 {
-	UASSERT(margin >= 0);
+	UASSERT(maxGraphDepth >= 0);
 	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
 	if(dbAccessTime)
 	{
@@ -861,9 +863,10 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	std::set<int> nextMargin;
 	nextMargin.insert(signatureId);
 	int m = 0;
-	while((margin == 0 || m < margin) && nextMargin.size())
+	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
 	{
-		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
+		// insert more recent first (priority to be loaded first from the database below if set)
+		curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
 		nextMargin.clear();
 
 		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
@@ -927,6 +930,73 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	return ids;
 }
 
+// return map<Id,sqrdDistance>, including signatureId
+std::map<int, float> Memory::getNeighborsIdRadius(
+		int signatureId,
+		float radius, // 0 means ignore radius
+		const std::map<int, Transform> & optimizedPoses,
+		int maxGraphDepth // 0 means infinite margin
+		) const
+{
+	UASSERT(maxGraphDepth >= 0);
+	UASSERT(uContains(optimizedPoses, signatureId));
+	UASSERT(signatureId > 0);
+	std::map<int, float> ids;
+	std::list<int> curentMarginList;
+	std::set<int> currentMargin;
+	std::set<int> nextMargin;
+	nextMargin.insert(signatureId);
+	int m = 0;
+	Transform referential = optimizedPoses.at(signatureId);
+	UASSERT(!referential.isNull());
+	float radiusSqrd = radius*radius;
+	std::map<int, float> savedRadius;
+	savedRadius.insert(std::make_pair(signatureId, 0));
+	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
+	{
+		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
+		nextMargin.clear();
+
+		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
+		{
+			if(ids.find(*jter) == ids.end())
+			{
+				//UDEBUG("Added %d with margin %d", *jter, m);
+				// Look up in STM/WM if all ids are here, if not... load them from the database
+				const Signature * s = this->getSignature(*jter);
+				std::map<int, Link> tmpLinks;
+				const std::map<int, Link> * links = &tmpLinks;
+				if(s)
+				{
+					ids.insert(std::pair<int, float>(*jter, savedRadius.at(*jter)));
+
+					links = &s->getLinks();
+				}
+
+				// links
+				for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
+				{
+					if(!uContains(ids, iter->first) &&
+						uContains(optimizedPoses, iter->first))
+					{
+						const Transform & t = optimizedPoses.at(iter->first);
+						UASSERT(!t.isNull());
+						float distanceSqrd = referential.getDistanceSquared(t);
+						if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
+						{
+							savedRadius.insert(std::make_pair(iter->first, distanceSqrd));
+							nextMargin.insert(iter->first);
+						}
+
+					}
+				}
+			}
+		}
+		++m;
+	}
+	return ids;
+}
+
 int Memory::getNextId()
 {
 	return ++_idCount;
@@ -1256,7 +1326,7 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
 {
 	UDEBUG("");
 	std::list<int> signaturesRemoved;
-	if(_vwd->isIncremental())
+	if(_vwd->isIncremental() && _vwd->getVisualWords().size())
 	{
 		int newWords = 0;
 		int wordsRemoved = 0;
@@ -1890,6 +1960,13 @@ Transform Memory::computeVisualTransform(
 					if(variance <= _bowEpipolarGeometryVar)
 					{
 						transform = cameraTransform.inverse();
+						if(_bowForce2D)
+						{
+							UDEBUG("Forcing 2D...");
+							float x,y,z,r,p,yaw;
+							transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
+							transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+						}
 					}
 					else
 					{
@@ -1990,6 +2067,22 @@ Transform Memory::computeVisualTransform(
 		}
 	}
 
+	if(!transform.isNull())
+	{
+		// verify if it is a 180 degree transform, well verify > 90
+		float roll,pitch,yaw;
+		transform.getEulerAngles(roll, pitch, yaw);
+		if(fabs(roll) > CV_PI/2 ||
+		   fabs(pitch) > CV_PI/2 ||
+		   fabs(yaw) > CV_PI/2)
+		{
+			transform.setNull();
+			msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+					roll, pitch, yaw);
+			UWARN(msg.c_str());
+		}
+	}
+
 	if(rejectedMsg)
 	{
 		*rejectedMsg = msg;
@@ -2295,7 +2388,8 @@ Transform Memory::computeIcpTransform(
 				}
 				else
 				{
-					correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
+					UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+							newS.id());
 				}
 
 				UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
@@ -2466,9 +2560,18 @@ Transform Memory::computeScanMatchingTransform(
 		UDEBUG("icpT=%s", icpT.prettyPrint().c_str());
 
 		// verify if there enough correspondences
-		float correspondencesRatio = float(correspondences)/float(newCloud->size());
+		float correspondencesRatio = 0.0f;
+		if(newS->getLaserScanMaxPts())
+		{
+			correspondencesRatio = float(correspondences)/float(newS->getLaserScanMaxPts());
+		}
+		else
+		{
+			UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+					newS->id());
+		}
 
-		UDEBUG("variance=%f, correspondences=%d/%d (%f%%)",
+		UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
 				variance?*variance:-1,
 				correspondences,
 				(int)newCloud->size(),
@@ -2479,16 +2582,20 @@ Transform Memory::computeScanMatchingTransform(
 			*inliers = correspondences;
 		}
 
+		//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
+		//pcl::io::savePCDFile("new.pcd", *newCloud, true);
+		//UWARN("local scan matching old.pcd, new.pcd saved!");
+		//if(!icpT.isNull())
+		//{
+		//	newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
+		//	pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
+		//	UWARN("local scan matching newFinal.pcd saved!");
+		//}
+
 		if(!icpT.isNull() && hasConverged &&
 		   correspondencesRatio >= _icp2CorrespondenceRatio)
 		{
 			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
-
-			//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
-			//pcl::io::savePCDFile("new.pcd", *newCloud, true);
-			//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
-			//pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
-			//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
 		}
 		else
 		{
@@ -2788,15 +2895,23 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 						fabs(y) > _rehearsalMaxDistance ||
 						fabs(z) > _rehearsalMaxDistance)) ||
 					(_rehearsalMaxAngle>0.0f && (
-					fabs(roll) > _rehearsalMaxAngle ||
-					fabs(pitch) > _rehearsalMaxAngle ||
-					fabs(yaw) > _rehearsalMaxAngle)))
+						fabs(roll) > _rehearsalMaxAngle ||
+						fabs(pitch) > _rehearsalMaxAngle ||
+						fabs(yaw) > _rehearsalMaxAngle)))
 				{
-					// if the robot has moved, transfer only weight
-					signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
-					sB->setWeight(0);
-					UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
-							signature->getWeight(), signature->id(), id, _rehearsalMaxDistance, _rehearsalMaxAngle);
+					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 previous
+						// signature because they are not merged
+						sB->setWeight(sB->getWeight()+1);
+						UINFO("Only updated weight to %d of %d (new=%d) because the robot has moved. (d=%f a=%f)",
+								sB->getWeight(), sB->id(), signature->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
+					}
 				}
 				else if(this->rehearsalMerge(id, signature->id()))
 				{
@@ -3017,7 +3132,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 			s->uncompressData();
 			r.setImageRaw(s->getImageRaw());
 			r.setDepthRaw(s->getDepthRaw());
-			r.setLaserScanRaw(s->getLaserScanRaw());
+			r.setLaserScanRaw(s->getLaserScanRaw(), s->getLaserScanMaxPts());
 		}
 		else
 		{
@@ -3368,7 +3483,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 		{
 			to->setImageCompressed(from->getImageCompressed());
 			to->setDepthCompressed(from->getDepthCompressed(), from->getFx(), from->getFy(), from->getCx(), from->getCy());
-			to->setLaserScanCompressed(from->getLaserScanCompressed());
+			to->setLaserScanCompressed(from->getLaserScanCompressed(), from->getLaserScanMaxPts());
 			to->setLocalTransform(from->getLocalTransform());
 		}
 
@@ -3933,7 +4048,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	{
 		s->setImageRaw(image);
 		s->setDepthRaw(depthOrRightImage);
-		s->setLaserScanRaw(laserScan);
+		s->setLaserScanRaw(laserScan, data.laserScanMaxPts());
 	}
 
 

corelib/src/Rtabmap.cpp

@@ -96,7 +96,7 @@ Rtabmap::Rtabmap() :
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
-	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
+	_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
 	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
 	_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
 	_databasePath(""),
@@ -385,7 +385,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
-	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
@@ -880,8 +880,9 @@ bool Rtabmap::process(const SensorData & data)
 	}
 	else
 	{
-		SensorData dataImageOnly(data.image(), data.id(), data.stamp(), data.userData());
-		if(!_memory->update(dataImageOnly, &statistics_))
+		SensorData dataWithoutOdom = data;
+		dataWithoutOdom.setPose(Transform(), 1, 1);
+		if(!_memory->update(dataWithoutOdom, &statistics_))
 		{
 			return false;
 		}
@@ -952,7 +953,7 @@ bool Rtabmap::process(const SensorData & data)
 			UASSERT(oldS != 0);
 			std::string rejectedMsg;
 			Transform guess = signature->getLinks().begin()->second.transform();
-			double variance = -1.0;
+			double variance = 1.0;
 			int inliers = 0;
 			float inliersRatio = 0;
 			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
@@ -964,7 +965,7 @@ bool Rtabmap::process(const SensorData & data)
 						oldId,
 						signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 						t.prettyPrint().c_str());
-				_memory->updateLink(signature->id(), oldId, t, 1, 1); // set Identify covariance
+				_memory->updateLink(signature->id(), oldId, t, variance, variance);
 			}
 			else
 			{
@@ -1304,53 +1305,50 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
 	//============================================================
-	std::set<int> retrievalLocalIds;
-	if(_rgbdSlamMode && _maxLocalRetrieved > 0)
+	std::list<int> retrievalLocalIds;
+	if(_rgbdSlamMode)
 	{
 		// Priority on locations on the planned path
 		if(_path.size())
 		{
 			updateGoalIndex();
 
-			if(_path.size())
+			float distanceSoFar = 0.0f;
+			// immunize all nodes after current node and
+			// retrieve nodes after current node in the maximum radius from the current node
+			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
-				float distanceSoFar = 0.0f;
-				// immunize all nodes after current node and
-				// retrieve nodes after current node in the maximum radius from the current node
-				for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
+				if(_localRadius > 0.0f && i != _pathCurrentIndex)
 				{
-					if(_localRadius > 0.0f && i != _pathCurrentIndex)
-					{
-						distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-					}
+					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
+				}
 
-					if(distanceSoFar <= _localRadius)
+				if(distanceSoFar <= _localRadius)
+				{
+					if(_memory->getSignature(_path[i].first) != 0)
 					{
-						if(_memory->getSignature(_path[i].first) != 0)
-						{
-							immunizedLocations.insert(_path[i].first);
-							UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
-						}
-						else if(retrievalLocalIds.size() < _maxLocalRetrieved)
-						{
-							UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-							retrievalLocalIds.insert(_path[i].first);
-							// retrieved locations are automatically immunized
-						}
+						immunizedLocations.insert(_path[i].first);
+						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 					}
-					else
+					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
 					{
-						UDEBUG("Stop on node %d (dist=%fm > %fm)",
-								_path[i].first, distanceSoFar, _localRadius);
-						break;
+						UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
+						retrievalLocalIds.push_back(_path[i].first);
+						// retrieved locations are automatically immunized
 					}
 				}
+				else
+				{
+					UDEBUG("Stop on node %d (dist=%fm > %fm)",
+							_path[i].first, distanceSoFar, _localRadius);
+					break;
+				}
 			}
 		}
-		else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+		if(retrievalLocalIds.size() < _maxLocalRetrieved)
 		{
 			// retrieval based on the nodes near the current pose
-			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, 0, _localRadius);
+			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
 			// sort by distance
 			std::multimap<float, int> nearNodesByDist;
 			for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
@@ -1366,17 +1364,41 @@ bool Rtabmap::process(const SensorData & data)
 				// If there is a change of direction, better to be retrieving
 				// 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;
+				for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
+					jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
 					++jter)
 				{
 					if(_memory->getSignature(jter->first) == 0)
 					{
 						UINFO("retrieval of node %d on local map", jter->first);
-						retrievalLocalIds.insert(jter->first);
+						retrievalLocalIds.push_back(jter->first);
 					}
 				}
 			}
+			// well, if the maximum retrieved is not reached, look for neighbors in database
+			if(retrievalLocalIds.size() < _maxLocalRetrieved)
+			{
+				std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
+				for(std::list<int>::iterator iter=retrievalLocalIds.begin();
+					iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++iter)
+				{
+					std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
+					for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
+						jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+						++jter)
+					{
+						if(_memory->getSignature(jter->first) == 0 &&
+						   retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
+						{
+							UINFO("retrieval of node %d on local map", jter->first);
+							retrievalLocalIds.push_back(jter->first);
+							retrievalLocalIdsSet.insert(jter->first);
+						}
+					}
+				}
+			}
+
 			// update Age of the close signatures (oldest the farthest)
 			for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
 			{
@@ -1488,7 +1510,6 @@ bool Rtabmap::process(const SensorData & data)
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
 				transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
-				variance = 1.0f; // ICP, set variance to 1
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1519,12 +1540,12 @@ bool Rtabmap::process(const SensorData & data)
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
 
-	int localSpaceClosuresAdded = 0;
+	int localSpaceClosuresAddedVisually = 0;
 	int localSpaceClosuresAddedByICPOnly = 0;
 	int lastLocalSpaceClosureId = 0;
 	int localSpacePaths = 0;
 	if(_localLoopClosureDetectionSpace &&
-	   !signature->getLaserScanCompressed().empty())
+	   _localRadius > 0)
 	{
 		if(_graphOptimizer->iterations() == 0)
 		{
@@ -1533,108 +1554,204 @@ bool Rtabmap::process(const SensorData & data)
 		else
 		{
 			//============================================================
-			// Scan matching LOCAL LOOP CLOSURE SPACE
+			// LOCAL LOOP CLOSURE SPACE
 			//============================================================
-			std::map<int, Transform> forwardPoses;
-			forwardPoses = this->getForwardWMPoses(
-					signature->id(),
-					0,
-					_localRadius,
-					_localDetectMaxDiffID);
 
-			std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
-			localSpacePaths = (int)forwardPaths.size();
-
-			for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
+			//
+			// 1) compare visually with nearest locations
+			//
+			float r = _localRadius;
+			if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius)
+			{
+				r = _localPathFilteringRadius;
+			}
+			std::map<int, float> nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
+			std::map<int, Transform> nearestPoses;
+			for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
+			{
+				nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+			}
+			// segment poses by paths, only one detection per path
+			std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
+			for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin(); iter!=nearestPaths.end(); ++iter)
 			{
 				std::map<int, Transform> & path = *iter;
 				UASSERT(path.size());
+				//find the nearest pose on the path
+				int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
+				UASSERT(nearestId > 0);
 
-				// only do local loop closure detection if there is no
-				// global loop closure already detected on this path
-				if(_loopClosureHypothesis.first == 0 || path.find(_loopClosureHypothesis.first) == path.end())
+				// nearest pose must not be linked to current location, and not in STM
+				if(!signature->hasLink(nearestId) &&
+				   _memory->getStMem().find(nearestId) == _memory->getStMem().end())
 				{
+					double variance = 1.0;
+					Transform transform;
+					if(_reextractLoopClosureFeatures)
+					{
+						ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
+						// override some parameters
+						uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
+						uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+						uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
+						uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
+						uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+						uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+						uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+						uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+						uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
+						uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
+
+						//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
+						//{
+						//	UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
+						//}
+
+						Memory memory(customParameters);
+
+						UTimer timeT;
+
+						// Add signatures
+						SensorData dataFrom = data;
+						dataFrom.setId(signature->id());
+						Signature tmpTo = _memory->getSignatureData(nearestId, true);
+						SensorData dataTo = tmpTo.toSensorData();
+						UDEBUG("timeTo = %fs", timeT.ticks());
+
+						if(dataFrom.isValid() &&
+						   dataFrom.isMetric() &&
+						   dataTo.isValid() &&
+						   dataTo.isMetric() &&
+						   dataFrom.id() != Memory::kIdInvalid &&
+						   tmpTo.id() != Memory::kIdInvalid)
+						{
+							memory.update(dataTo);
+							UDEBUG("timeUpTo = %fs", timeT.ticks());
+							memory.update(dataFrom);
+							UDEBUG("timeUpFrom = %fs", timeT.ticks());
+
+							transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
+							UDEBUG("timeTransform = %fs", timeT.ticks());
+						}
+						else
+						{
+							// Fallback to normal way (raw data not kept in database...)
+							UWARN("Loop closure: Some images not found in memory for re-extracting "
+								  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
+							transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
+						}
+					}
+					else
+					{
+						transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
+					}
+					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
+					{
+						transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
+					}
+					if(!transform.isNull())
+					{
+						UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
+								signature->id(),
+								nearestId,
+								transform.prettyPrint().c_str());
+						_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
+
+						// 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();
+						++localSpaceClosuresAddedVisually;
+						lastLocalSpaceClosureId = nearestId;
+					}
+				}
+			}
+
+			//
+			// 2) compare locally with nearest locations by scan matching
+			//
+			if( !signature->getLaserScanCompressed().empty())
+			{
+				std::map<int, Transform> forwardPoses;
+				forwardPoses = this->getForwardWMPoses(
+						signature->id(),
+						0,
+						_localRadius,
+						_localDetectMaxGraphDepth);
+
+				std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
+				localSpacePaths = (int)forwardPaths.size();
+
+				for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
+				{
+					std::map<int, Transform> & path = *iter;
+					UASSERT(path.size());
+
 					//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)
+					// nearest pose must be close and not linked to current location
+					if(!signature->hasLink(nearestId) &&
+					   (_localPathFilteringRadius <= 0.0f ||
+						_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
 					{
-						// 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)
+						// Assemble scans in the path and do ICP only
+						if(_localPathOdomPosesUsed)
 						{
-							transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
-							variance = 1.0f; // ICP, set variance to 1
-						}
-						if(transform.isNull())
-						{
-							// 2) Assemble scans in the path and do ICP only
-							if(_localPathOdomPosesUsed)
+							//optimize the path's poses locally
+							path = optimizeGraph(nearestId, uKeys(path), false);
+							// transform local poses in optimized graph referential
+							Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+							for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
 							{
-								//optimize the path's poses locally
-								path = optimizeGraph(nearestId, uKeys(path), false);
-								// transform local poses in optimized graph referential
-								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
-								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
+								jter->second = t * jter->second;
+							}
+						}
+						if(_localPathFilteringRadius > 0.0f)
+						{
+							// path filtering
+							std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
+							// make sure the nearest and farthest poses are still here
+							filteredPath.insert(*path.find(nearestId));
+							filteredPath.insert(*path.begin());
+							filteredPath.insert(*path.rbegin());
+							path = filteredPath;
+						}
+
+						if(path.size() > 2) // more than current+nearest
+						{
+							// 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 transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0);
+								if(!transform.isNull())
 								{
-									jter->second = t * jter->second;
+									UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
+											signature->id(),
+											nearestId,
+											transform.prettyPrint().c_str());
+									// set Identify covariance for laser scan matching only
+									_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
+
+									++localSpaceClosuresAddedByICPOnly;
+
+									// no local loop closure added visually
+									if(localSpaceClosuresAddedVisually == 0)
+									{
+										// 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();
+										lastLocalSpaceClosureId = nearestId;
+									}
 								}
 							}
-							if(_localPathFilteringRadius > 0.0f)
-							{
-								// path filtering
-								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
-								// make sure the nearest and farthest poses are still here
-								filteredPath.insert(*path.find(nearestId));
-								filteredPath.insert(*path.begin());
-								filteredPath.insert(*path.rbegin());
-								path = filteredPath;
-							}
-
-							if(path.size() > 2) // more than current+nearest
-							{
-								// 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);
 						}
 					}
 				}
@@ -1762,7 +1879,7 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
@@ -1861,6 +1978,7 @@ bool Rtabmap::process(const SensorData & data)
 			if(_path.size())
 			{
 				statistics_.setLocalPath(this->getPathNextNodes());
+				statistics_.setCurrentGoalId(this->getPathCurrentGoalId());
 			}
 		}
 
@@ -1977,6 +2095,7 @@ bool Rtabmap::process(const SensorData & data)
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());
 
 		if(_rgbdSlamMode)
 		{
@@ -2166,25 +2285,27 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		UDEBUG("");
 		const Signature * fromS = _memory->getSignature(fromId);
 		UASSERT(fromS != 0);
+		UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(_optimizedPoses.size());
 		std::vector<int> ids(_optimizedPoses.size());
 		int oi = 0;
 		const std::set<int> & stm = _memory->getStMem();
-		//get margins
-		std::map<int, int> margins;
+		//get distances
+		std::map<int, float> foundIds;
 		if(maxDiffID > 0)
 		{
-			margins = _memory->getNeighborsId(fromId, maxDiffID, 0, true, false);
+			foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxDiffID);
 		}
+		float radiusSqrd = radius * radius;
 		for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
 			if(iter->first != fromId)
 			{
-				// Only locations in Working Memory not too far from the current node (so inside the margin)
-				bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
-				if(stm.find(iter->first) == stm.end() && diffIdOk)
+				if(stm.find(iter->first) == stm.end() &&
+				   uContains(foundIds, iter->first) &&
+				   (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
 				{
 					(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 					ids[oi++] = iter->first;
@@ -2195,7 +2316,6 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		cloud->resize(oi);
 		ids.resize(oi);
 
-		UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
 		Transform fromT = _optimizedPoses.at(fromId);
 
 		if(cloud->size())
@@ -2904,24 +3024,17 @@ void Rtabmap::updateGoalIndex()
 		{
 			//Always check if the farthest node is accessible in local map (max to local space radius if set)
 			int goalIndex = _pathCurrentIndex;
-			float distanceSoFar = 0.0f;
+			float distanceFromCurrentNode = 0.0f;
 			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
 				if(uContains(_optimizedPoses, _path[i].first))
 				{
 					if(_localRadius > 0.0f)
 					{
-						if(i == _pathCurrentIndex)
-						{
-							distanceSoFar += _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
-						}
-						else
-						{
-							distanceSoFar += _optimizedPoses.at(_path[i-1].first).getDistance(_optimizedPoses.at(_path[i].first));
-						}
+						distanceFromCurrentNode = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
 					}
 
-					if(distanceSoFar <= _localRadius)
+					if(distanceFromCurrentNode <= _localRadius)
 					{
 						goalIndex = i;
 					}

corelib/src/Statistics.cpp

@@ -42,7 +42,8 @@ Statistics::Statistics() :
 	_extended(0),
 	_refImageId(0),
 	_loopClosureId(0),
-	_localLoopClosureId(0)
+	_localLoopClosureId(0),
+	_currentGoalId(0)
 {
 	_defaultDataInitialized = true;
 }

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -120,7 +120,8 @@ private:
 				rtabmap::ImageView * view,
 				rtabmap::CloudViewer * view3D,
 				QLabel * labelId,
-				bool updateConstraintView = true);
+				QLabel * labelMapId,
+				bool updateConstraintView);
 	void updateStereo(const Signature * data);
 	void updateWordsMatching();
 	void updateConstraintView(
@@ -139,8 +140,8 @@ private:
 	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
 			const std::multimap<int, rtabmap::Link> & edgeConstraints);
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
-	void refineConstraint(int from, int to, bool updateGraph);
-	void refineConstraintVisually(int from, int to, bool updateGraph);
+	void refineConstraint(int from, int to,  bool silent, bool updateGraph);
+	void refineConstraintVisually(int from, int to,  bool silent, bool updateGraph);
 	bool addConstraint(int from, int to, bool silent, bool updateGraph);
 
 private:

guilib/include/rtabmap/gui/MainWindow.h

@@ -157,8 +157,6 @@ private slots:
 	void downloadAllClouds();
 	void downloadPoseGraph();
 	void clearTheCache();
-	void saveFigures();
-	void loadFigures();
 	void openPreferences();
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
@@ -218,6 +216,9 @@ private:
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceMenu();
+	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
+	void saveFigures();
+	void loadFigures();
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
 			const std::map<int, Transform> & poses,

guilib/src/DatabaseViewer.cpp

@@ -237,6 +237,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->checkBox_icp_p2plane, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->doubleSpinBox_icp_minCorrespondenceRatio, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	// Visual parameters
 	connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->comboBox_featureType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
@@ -354,6 +355,7 @@ void DatabaseViewer::readSettings()
 	ui_->checkBox_icp_p2plane->setChecked(settings.value("point2place", ui_->checkBox_icp_p2plane->isChecked()).toBool());
 	ui_->spinBox_icp_normalKSearch->setValue(settings.value("normalKSearch", ui_->spinBox_icp_normalKSearch->value()).toInt());
 	ui_->checkBox_icp_2d->setChecked(settings.value("icp2d", ui_->checkBox_icp_2d->isChecked()).toBool());
+	ui_->doubleSpinBox_icp_minCorrespondenceRatio->setValue(settings.value("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()).toDouble());
 	settings.endGroup();
 
 	// Visual parameters
@@ -429,6 +431,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("point2place", ui_->checkBox_icp_p2plane->isChecked());
 	settings.setValue("normalKSearch", ui_->spinBox_icp_normalKSearch->value());
 	settings.setValue("icp2d", ui_->checkBox_icp_2d->isChecked());
+	settings.setValue("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value());
 	settings.endGroup();
 
 	// save Visual parameters
@@ -678,6 +681,8 @@ void DatabaseViewer::exportDatabase()
 			QList<int> ids;
 
 			double previousStamp = 0;
+			std::vector<double> delays(ids_.size());
+			int oi=0;
 			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
 				Transform odomPose;
@@ -697,6 +702,10 @@ void DatabaseViewer::exportDatabase()
 						{
 							ids.push_back(ids_[i]);
 						}
+						if(previousStamp && stamp)
+						{
+							delays[oi++] = stamp - previousStamp;
+						}
 						previousStamp = stamp;
 					}
 					if(sessionExported >= 0 && mapId > sessionExported)
@@ -705,11 +714,14 @@ void DatabaseViewer::exportDatabase()
 					}
 				}
 			}
+			delays.resize(oi);
+
 			if(recorder.init(path, false))
 			{
-				rtabmap::DetailedProgressDialog progressDialog(this);
-				progressDialog.setMaximumSteps(ids.size());
-				progressDialog.show();
+				rtabmap::DetailedProgressDialog * progressDialog = new rtabmap::DetailedProgressDialog(this);
+				progressDialog->setAttribute(Qt::WA_DeleteOnClose);
+				progressDialog->setMaximumSteps(ids.size());
+				progressDialog->show();
 
 				for(int i=0; i<ids.size(); ++i)
 				{
@@ -723,12 +735,17 @@ void DatabaseViewer::exportDatabase()
 					}
 					recorder.addData(sensorData);
 
-					progressDialog.appendText(tr("Exported node %1").arg(id));
-					progressDialog.incrementStep();
+					progressDialog->appendText(tr("Exported node %1").arg(id));
+					progressDialog->incrementStep();
 					QApplication::processEvents();
 				}
-				progressDialog.setValue(progressDialog.maximumSteps());
-				progressDialog.appendText("Export finished!");
+				progressDialog->setValue(progressDialog->maximumSteps());
+				if(delays.size())
+				{
+					progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
+							.arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
+				}
+				progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
 			}
 			else
 			{
@@ -1261,7 +1278,7 @@ void DatabaseViewer::refineAllNeighborLinks()
 		{
 			int from = neighborLinks_[i].from();
 			int to = neighborLinks_[i].to();
-			this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
+			this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
 			progressDialog.incrementStep();
@@ -1286,7 +1303,7 @@ void DatabaseViewer::refineAllLoopClosureLinks()
 		{
 			int from = loopLinks_[i].from();
 			int to = loopLinks_[i].to();
-			this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), false);
+			this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
 			progressDialog.incrementStep();
@@ -1311,7 +1328,7 @@ void DatabaseViewer::refineVisuallyAllNeighborLinks()
 		{
 			int from = neighborLinks_[i].from();
 			int to = neighborLinks_[i].to();
-			this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
+			this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
 			progressDialog.incrementStep();
@@ -1336,7 +1353,7 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
 		{
 			int from = loopLinks_[i].from();
 			int to = loopLinks_[i].to();
-			this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), false);
+			this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
 			progressDialog.incrementStep();
@@ -1360,7 +1377,9 @@ void DatabaseViewer::sliderAValueChanged(int value)
 			ui_->label_stampA,
 			ui_->graphicsView_A,
 			ui_->widget_cloudA,
-			ui_->label_idA);
+			ui_->label_idA,
+			ui_->label_mapA,
+			true);
 }
 
 void DatabaseViewer::sliderBValueChanged(int value)
@@ -1374,7 +1393,9 @@ void DatabaseViewer::sliderBValueChanged(int value)
 			ui_->label_stampB,
 			ui_->graphicsView_B,
 			ui_->widget_cloudB,
-			ui_->label_idB);
+			ui_->label_idB,
+			ui_->label_mapB,
+			true);
 }
 
 void DatabaseViewer::update(int value,
@@ -1387,6 +1408,7 @@ void DatabaseViewer::update(int value,
 						rtabmap::ImageView * view,
 						rtabmap::CloudViewer * view3D,
 						QLabel * labelId,
+						QLabel * labelMapId,
 						bool updateConstraintView)
 {
 	UTimer timer;
@@ -1395,6 +1417,7 @@ void DatabaseViewer::update(int value,
 	labelChildren->clear();
 	weight->clear();
 	label->clear();
+	labelMapId->clear();
 	stamp->clear();
 	QRectF rect;
 	if(value >= 0 && value < ids_.size())
@@ -1520,11 +1543,7 @@ void DatabaseViewer::update(int value,
 
 		if(mapId>=0)
 		{
-			labelId->setText(QString("%1 [%2]").arg(id).arg(mapId));
-		}
-		else
-		{
-			labelId->setText(QString::number(id));
+			labelMapId->setText(QString::number(mapId));
 		}
 	}
 	else
@@ -1849,16 +1868,19 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
 // only called when ui_->checkBox_showOptimized state changed
 void DatabaseViewer::updateConstraintView()
 {
-	Link link = this->findActiveLink(ui_->horizontalSlider_A->value(), ui_->horizontalSlider_B->value());
-	if(link.isValid())
+	if(ids_.size())
 	{
-		if(link.type() == Link::kNeighbor)
+		Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
+		if(link.isValid())
 		{
-			this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
-		}
-		else
-		{
-			this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+			if(link.type() == Link::kNeighbor)
+			{
+				this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
+			}
+			else
+			{
+				this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+			}
 		}
 	}
 }
@@ -1934,6 +1956,7 @@ void DatabaseViewer::updateConstraintView(
 					ui_->graphicsView_A,
 					ui_->widget_cloudA,
 					ui_->label_idA,
+					ui_->label_mapA,
 					false); // don't update constraints view!
 		this->update(idToIndex_.value(link.to()),
 					ui_->label_indexB,
@@ -1945,6 +1968,7 @@ void DatabaseViewer::updateConstraintView(
 					ui_->graphicsView_B,
 					ui_->widget_cloudB,
 					ui_->label_idB,
+					ui_->label_mapB,
 					false); // don't update constraints view!
 	}
 
@@ -2105,10 +2129,18 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
 			}
+			else
+			{
+				ui_->constraintsViewer->removeCloud("scan0");
+			}
 			if(scanB->size())
 			{
 				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
 			}
+			else
+			{
+				ui_->constraintsViewer->removeCloud("scan1");
+			}
 		}
 		else
 		{
@@ -2116,10 +2148,18 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->addOrUpdateCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
 			}
+			else
+			{
+				ui_->constraintsViewer->removeCloud("scan0");
+			}
 			if(scanTo->size())
 			{
 				ui_->constraintsViewer->addOrUpdateCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
 			}
+			else
+			{
+				ui_->constraintsViewer->removeCloud("scan1");
+			}
 		}
 
 		//update cordinate
@@ -2451,10 +2491,10 @@ void DatabaseViewer::refineConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraint(from, to, true);
+	refineConstraint(from, to, false, true);
 }
 
-void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
+void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool updateGraph)
 {
 	if(from == to)
 	{
@@ -2501,6 +2541,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
+	float correspondenceRatio = 0.0f;
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
@@ -2529,6 +2570,18 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 					   &hasConverged,
 					   &variance,
 					   &correspondences);
+
+				if(!transform.isNull())
+				{
+					if(dataTo.getLaserScanMaxPts())
+					{
+						correspondenceRatio =  float(correspondences)/float(dataTo.getLaserScanMaxPts());
+					}
+					else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
+					{
+						UWARN("Laser scan max pts not set, but correspondence ratio is set!");
+					}
+				}
 			}
 		}
 	}
@@ -2639,53 +2692,75 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 					&hasConverged,
 					&variance,
 					&correspondences);
+
+			correspondenceRatio = float(correspondences)/float(depthB.total());
 		}
 	}
 
 	if(hasConverged && !transform.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
-
-		bool updated = false;
-		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
-		while(iter != linksRefined_.end() && iter->first == currentLink.from())
+		if(correspondenceRatio < ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
 		{
-			if(iter->second.to() == currentLink.to() &&
-			   iter->second.type() == currentLink.type())
+			if(!silent)
 			{
-				iter->second = newLink;
-				updated = true;
-				break;
-			}
-			++iter;
-		}
-		if(!updated)
-		{
-			linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
-
-			if(updateGraph)
-			{
-				this->updateGraphView();
+				QMessageBox::warning(this,
+								tr("Refine link"),
+								tr("Cannot find a transformation between nodes %1 and %2, correspondence ratio too low (%3).")
+								.arg(from).arg(to).arg(correspondenceRatio));
 			}
 		}
-
-		if(ui_->dockWidget_constraints->isVisible())
+		else
 		{
-			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
-			scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, transform);
-			this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
+
+			Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
+
+			bool updated = false;
+			std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
+			while(iter != linksRefined_.end() && iter->first == currentLink.from())
+			{
+				if(iter->second.to() == currentLink.to() &&
+				   iter->second.type() == currentLink.type())
+				{
+					iter->second = newLink;
+					updated = true;
+					break;
+				}
+				++iter;
+			}
+			if(!updated)
+			{
+				linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
+
+				if(updateGraph)
+				{
+					this->updateGraphView();
+				}
+			}
+
+			if(ui_->dockWidget_constraints->isVisible())
+			{
+				cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
+				scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, transform);
+				this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
+			}
 		}
 	}
+	else if(!silent)
+	{
+		QMessageBox::warning(this,
+				tr("Refine link"),
+				tr("Cannot find a transformation between nodes %1 and %2").arg(from).arg(to));
+	}
 }
 
 void DatabaseViewer::refineConstraintVisually()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraintVisually(from, to, true);
+	refineConstraintVisually(from, to, false, true);
 }
 
-void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph)
+void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, bool updateGraph)
 {
 	if(from == to)
 	{
@@ -2751,6 +2826,14 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
 
 	if(!t.isNull())
 	{
+		if(ui_->checkBox_visual_2d->isChecked())
+		{
+			// We are 2D here, make sure the guess has only YAW rotation
+			float x,y,z,r,p,yaw;
+			t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
+			t = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+		}
+
 		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
 
 		bool updated = false;
@@ -2780,6 +2863,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
 			this->updateConstraintView(newLink);
 		}
 	}
+	else if(!silent)
+	{
+		QMessageBox::warning(this,
+				tr("Add link"),
+				tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+	}
 }
 
 void DatabaseViewer::addConstraint()
@@ -2860,16 +2949,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
 		}
 
-		if(t.isNull())
-		{
-			if(!silent)
-			{
-				QMessageBox::warning(this,
-						tr("Add link"),
-						tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
-			}
-		}
-		else
+		if(!t.isNull())
 		{
 			if(ui_->checkBox_visual_2d->isChecked())
 			{
@@ -2890,6 +2970,12 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			}
 			updateSlider = true;
 		}
+		else if(!silent)
+		{
+			QMessageBox::warning(this,
+					tr("Add link"),
+					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
+		}
 	}
 	else if(containsLink(linksRemoved_, from, to))
 	{

guilib/src/DetailedProgressDialog.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QCheckBox>
 #include <QtCore/QTimer>
 #include <QtCore/QTime>
+#include <QScrollBar>
 #include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap {
@@ -88,6 +89,7 @@ void DetailedProgressDialog::appendText(const QString & text, const QColor & col
 	QString html = tr("<html><font color=\"#999999\">%1 </font><font color=\"%2\">%3</font></html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text);
 	_detailedText->append(html);
 	_detailedText->ensureCursorVisible();
+	_detailedText->horizontalScrollBar()->setSliderPosition(0);
 }
 void DetailedProgressDialog::setValue(int value)
 {

guilib/src/GraphViewer.cpp

@@ -158,6 +158,7 @@ private:
 GraphViewer::GraphViewer(QWidget * parent) :
 		QGraphicsView(parent),
 		_nodeColor(Qt::blue),
+		_currentGoalColor(Qt::darkMagenta),
 		_neighborColor(Qt::blue),
 		_loopClosureColor(Qt::red),
 		_loopClosureLocalColor(Qt::yellow),
@@ -170,7 +171,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_linkWidth(0),
 		_gridMap(0),
 		_referential(0),
-		_gridCellSize(0.0f)
+		_gridCellSize(0.0f),
+		_localRadius(0)
 {
 	this->setScene(new QGraphicsScene(this));
 	this->setDragMode(QGraphicsView::ScrollHandDrag);
@@ -203,12 +205,19 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	item->setParentItem(_root);
 	_referential->addToGroup(item);
 
+	_localRadius = this->scene()->addEllipse(-0.0001,-0.0001,0.0001,0.0001);
+	_localRadius->setZValue(1);
+	_localRadius->setParentItem(_root);
+	_localRadius->setVisible(false);
+	_localRadius->setPen(QPen(Qt::DashLine));
 
 	_gridMap = this->scene()->addPixmap(QPixmap());
 	_gridMap->setZValue(0);
 	_gridMap->setParentItem(_root);
 
 	this->restoreDefaults();
+
+	this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
 }
 
 GraphViewer::~GraphViewer()
@@ -247,7 +256,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 				const Transform & pose = iter->second;
 				NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius);
 				this->scene()->addItem(item);
-				item->setZValue(2);
+				item->setZValue(20);
 				item->setColor(_nodeColor);
 				item->setParentItem(_root);
 				_nodeItems.insert(iter->first, item);
@@ -334,7 +343,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 					item->setColor(_loopClosureColor);
 				}
 				this->scene()->addItem(item);
-				item->setZValue(1);
+				item->setZValue(10);
 				item->setParentItem(_root);
 				_linkItems.insert(idFrom, item);
 			}
@@ -384,6 +393,7 @@ void GraphViewer::updateReferentialPosition(const Transform & t)
 {
 	QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
 	_referential->setTransform(qt);
+	_localRadius->setTransform(qt);
 
 	this->ensureVisible(_referential);
 }
@@ -456,13 +466,31 @@ void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > &
 			item->setPen(p);
 			item->setColor(_globalPathColor);
 			this->scene()->addItem(item);
-			item->setZValue(1);
+			item->setZValue(15);
 			item->setParentItem(_root);
 			_globalPathLinkItems.insert(idFrom, item);
 		}
 	}
 }
 
+void GraphViewer::setCurrentGoalID(int id)
+{
+	NodeItem * node = _nodeItems.value(id, 0);
+	if(node)
+	{
+		node->setColor(_currentGoalColor);
+	}
+	else
+	{
+		UWARN("Curent goal %d not found in the graph", id);
+	}
+}
+
+void GraphViewer::setLocalRadius(float radius)
+{
+	_localRadius->setRect(-radius, -radius, radius*2, radius*2);
+}
+
 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
 {
 	if(localPath.size() > 1)
@@ -497,6 +525,7 @@ void GraphViewer::clearGraph()
 	qDeleteAll(_globalPathLinkItems);
 	_globalPathLinkItems.clear();
 	_referential->resetTransform();
+	_localRadius->resetTransform();
 	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
 }
 
@@ -530,6 +559,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("node_radius", (double)this->getNodeRadius());
 	settings.setValue("link_width", (double)this->getLinkWidth());
 	settings.setValue("node_color", this->getNodeColor());
+	settings.setValue("current_goal_color", this->getCurrentGoalColor());
 	settings.setValue("neighbor_color", this->getNeighborColor());
 	settings.setValue("global_color", this->getGlobalLoopClosureColor());
 	settings.setValue("local_color", this->getLocalLoopClosureColor());
@@ -540,6 +570,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("grid_visible", this->isGridMapVisible());
 	settings.setValue("origin_visible", this->isOriginVisible());
 	settings.setValue("referential_visible", this->isReferentialVisible());
+	settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -555,6 +586,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setNodeRadius(settings.value("node_radius", this->getNodeRadius()).toDouble());
 	this->setLinkWidth(settings.value("link_width", this->getLinkWidth()).toDouble());
 	this->setNodeColor(settings.value("node_color", this->getNodeColor()).value<QColor>());
+	this->setCurrentGoalColor(settings.value("current_goal_color", this->getCurrentGoalColor()).value<QColor>());
 	this->setNeighborColor(settings.value("neighbor_color", this->getNeighborColor()).value<QColor>());
 	this->setGlobalLoopClosureColor(settings.value("global_color", this->getGlobalLoopClosureColor()).value<QColor>());
 	this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
@@ -565,6 +597,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
 	this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool());
 	this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool());
+	this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -583,6 +616,10 @@ bool GraphViewer::isReferentialVisible() const
 {
 	return _referential->isVisible();
 }
+bool GraphViewer::isLocalRadiusVisible() const
+{
+	return _localRadius->isVisible();
+}
 
 void GraphViewer::setWorkingDirectory(const QString & path)
 {
@@ -619,6 +656,10 @@ void GraphViewer::setNodeColor(const QColor & color)
 		iter.value()->setColor(_nodeColor);
 	}
 }
+void GraphViewer::setCurrentGoalColor(const QColor & color)
+{
+	_currentGoalColor = color;
+}
 void GraphViewer::setNeighborColor(const QColor & color)
 {
 	_neighborColor = color;
@@ -699,6 +740,10 @@ void GraphViewer::setReferentialVisible(bool visible)
 {
 	_referential->setVisible(visible);
 }
+void GraphViewer::setLocalRadiusVisible(bool visible)
+{
+	_localRadius->setVisible(visible);
+}
 
 void GraphViewer::restoreDefaults()
 {
@@ -741,7 +786,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	menu.addSeparator();
 
 	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
+	QAction * aChangeCurrentGoalColor = menu.addAction(createIcon(_currentGoalColor), tr("Set current goal color..."));
 	aChangeNodeColor->setIconVisibleInMenu(true);
+	aChangeCurrentGoalColor->setIconVisibleInMenu(true);
 
 	// Links
 	QMenu * menuLink = menu.addMenu(tr("Set link color..."));
@@ -774,6 +821,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aShowHideGridMap;
 	QAction * aShowHideOrigin;
 	QAction * aShowHideReferential;
+	QAction * aShowHideLocalRadius;
 	if(_gridMap->isVisible())
 	{
 		aShowHideGridMap = menu.addAction(tr("Hide grid map"));
@@ -798,6 +846,14 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideReferential = menu.addAction(tr("Show current referential"));
 	}
+	if(_localRadius->isVisible())
+	{
+		aShowHideLocalRadius = menu.addAction(tr("Hide local radius"));
+	}
+	else
+	{
+		aShowHideLocalRadius = menu.addAction(tr("Show local radius"));
+	}
 	menu.addSeparator();
 	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
@@ -869,6 +925,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		return; // without emitting configChanged
 	}
 	else if(r == aChangeNodeColor ||
+			r == aChangeCurrentGoalColor ||
 			r == aChangeNeighborColor ||
 			r == aChangeGlobalLoopColor ||
 			r == aChangeLocalLoopColor ||
@@ -882,6 +939,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _nodeColor;
 		}
+		else if(r == aChangeCurrentGoalColor)
+		{
+			color = _currentGoalColor;
+		}
 		else if(r == aChangeGlobalLoopColor)
 		{
 			color = _loopClosureColor;
@@ -918,6 +979,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setNodeColor(color);
 			}
+			else if(r == aChangeCurrentGoalColor)
+			{
+				this->setCurrentGoalColor(color);
+			}
 			else if(r == aChangeGlobalLoopColor)
 			{
 				this->setGlobalLoopClosureColor(color);
@@ -978,6 +1043,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		this->setReferentialVisible(!this->isReferentialVisible());
 	}
+	else if(r == aShowHideLocalRadius)
+	{
+		this->setLocalRadiusVisible(!this->isLocalRadiusVisible());
+	}
 	else if(r == aRestoreDefaults)
 	{
 		this->restoreDefaults();

guilib/src/GraphViewer.h

@@ -59,6 +59,8 @@ public:
 	void updatePosterior(const std::map<int, float> & posterior);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
+	void setCurrentGoalID(int id);
+	void setLocalRadius(float radius);
 	void clearGraph();
 	void clearMap();
 	void clearPosterior();
@@ -72,6 +74,7 @@ public:
 	float getNodeRadius() const {return _nodeRadius;}
 	float getLinkWidth() const {return _linkWidth;}
 	const QColor & getNodeColor() const {return _nodeColor;}
+	const QColor & getCurrentGoalColor() const {return _currentGoalColor;}
 	const QColor & getNeighborColor() const {return _neighborColor;}
 	const QColor & getGlobalLoopClosureColor() const {return _loopClosureColor;}
 	const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
@@ -82,12 +85,14 @@ public:
 	bool isGridMapVisible() const;
 	bool isOriginVisible() const;
 	bool isReferentialVisible() const;
+	bool isLocalRadiusVisible() const;
 
 	// setters
 	void setWorkingDirectory(const QString & path);
 	void setNodeRadius(float radius);
 	void setLinkWidth(float width);
 	void setNodeColor(const QColor & color);
+	void setCurrentGoalColor(const QColor & color);
 	void setNeighborColor(const QColor & color);
 	void setGlobalLoopClosureColor(const QColor & color);
 	void setLocalLoopClosureColor(const QColor & color);
@@ -98,6 +103,7 @@ public:
 	void setGridMapVisible(bool visible);
 	void setOriginVisible(bool visible);
 	void setReferentialVisible(bool visible);
+	void setLocalRadiusVisible(bool visible);
 
 signals:
 	void configChanged();
@@ -112,6 +118,7 @@ protected:
 private:
 	QString _workingDirectory;
 	QColor _nodeColor;
+	QColor _currentGoalColor;
 	QColor _neighborColor;
 	QColor _loopClosureColor;
 	QColor _loopClosureLocalColor;
@@ -129,6 +136,7 @@ private:
 	QGraphicsItemGroup * _referential;
 	QGraphicsItemGroup * _originReferential;
 	float _gridCellSize;
+	QGraphicsEllipseItem * _localRadius;
 };
 
 } /* namespace rtabmap */

guilib/src/ImageView.cpp

@@ -529,22 +529,14 @@ void ImageView::updateOpacity()
 		if(_imageItem->isVisible() && _imageDepthItem->isVisible())
 		{
 			QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
-			QGraphicsOpacityEffect * effect2 = new QGraphicsOpacityEffect();
 			effect->setOpacity(0.5);
-			effect2->setOpacity(0.5);
-			_imageItem->setGraphicsEffect(effect);
-			_imageDepthItem->setGraphicsEffect(effect2);
+			_imageDepthItem->setGraphicsEffect(effect);
 		}
 		else
 		{
-			_imageItem->setGraphicsEffect(0);
 			_imageDepthItem->setGraphicsEffect(0);
 		}
 	}
-	else if(_imageItem)
-	{
-		_imageItem->setGraphicsEffect(0);
-	}
 	else if(_imageDepthItem)
 	{
 		_imageDepthItem->setGraphicsEffect(0);

guilib/src/MainWindow.cpp

@@ -369,9 +369,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
 	this->updateSelectSourceMenu();
 
-	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
-	connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures()));
-
 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
 
 	QActionGroup * modeGrp = new QActionGroup(this);
@@ -461,6 +458,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 			_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
 		}
 	}
+	this->loadFigures();
+	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
 
 	// update loop closure viewer parameters
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
@@ -1201,6 +1200,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			// clear the global path if set (goal reached)
 			_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
 		}
+		// update current goal id
+		if(stat.currentGoalId() > 0)
+		{
+			_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId());
+		}
 
 		UDEBUG("");
 	}
@@ -1986,7 +1990,7 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
 		_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);
+				10000);
 	}
 }
 
@@ -2074,6 +2078,11 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 }
 
 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
+{
+	applyPrefSettings(parameters, true); //post parameters
+}
+
+void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
 {
 	ULOGGER_DEBUG("");
 	if(parameters.size())
@@ -2098,7 +2107,10 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 					QMessageBox::information(this, tr("Working memory changed"), tr("The working directory can't be changed while the detector is running. This will be applied when the detector will stop."));
 				}
 			}
-			this->post(new ParamEvent(parametersModified));
+			if(postParamEvent)
+			{
+				this->post(new ParamEvent(parametersModified));
+			}
 		}
 
 		if(_state != kMonitoring && _state != kMonitoringPaused &&
@@ -2122,6 +2134,12 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 		{
 			_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
 		}
+
+		// update graph view parameters
+		if(uContains(parameters, Parameters::kRGBDLocalRadius()))
+		{
+			_ui->graphicsView_graphView->setLocalRadius(uStr2Float(parameters.at(Parameters::kRGBDLocalRadius())));
+		}
 	}
 
 	//update ui
@@ -2380,6 +2398,7 @@ void MainWindow::saveConfigGUI()
 	_preferencesDialog->saveWidgetState(_postProcessingDialog);
 	_preferencesDialog->saveWidgetState(_ui->graphicsView_graphView);
 	_preferencesDialog->saveSettings();
+	this->saveFigures();
 	this->setWindowModified(false);
 }
 
@@ -2426,6 +2445,7 @@ void MainWindow::newDatabase()
 	}
 	_newDatabasePath = databasePath.c_str();
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, databasePath, 0, _preferencesDialog->getAllParameters()));
+	applyPrefSettings(_preferencesDialog->getAllParameters(), false);
 }
 
 void MainWindow::openDatabase()
@@ -2446,6 +2466,7 @@ void MainWindow::openDatabase()
 		_openedDatabasePath = path;
 		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdInit, path.toStdString(), 0, _preferencesDialog->getAllParameters()));
 	}
+	applyPrefSettings(_preferencesDialog->getAllParameters(), false);
 }
 
 bool MainWindow::closeDatabase()
@@ -3854,20 +3875,6 @@ void MainWindow::saveFigures()
 	QStringList curveNames;
 	_ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
 
-	if(curvesPerFigure.size() == 0)
-	{
-		QMessageBox msgBox;
-		msgBox.setText("There is no figure shown.");
-		msgBox.setInformativeText("Do you want to save anyway ? (this will erase the previous saved configuration)");
-		msgBox.setStandardButtons(QFlags<QMessageBox::StandardButton>(QMessageBox::Save | QMessageBox::Cancel));
-		msgBox.setDefaultButton(QMessageBox::Cancel);
-		int ret = msgBox.exec();
-		if(ret == QMessageBox::Cancel)
-		{
-			return;
-		}
-	}
-
 	QStringList curvesPerFigureStr;
 	for(int i=0; i<curvesPerFigure.size(); ++i)
 	{
@@ -3886,25 +3893,28 @@ void MainWindow::loadFigures()
 	QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
 	QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
 
-	QStringList curvesPerFigureList = curvesPerFigure.split(" ");
-	QStringList curvesNamesList = curveNames.split(" ");
-
-	int j=0;
-	for(int i=0; i<curvesPerFigureList.size(); ++i)
+	if(!curvesPerFigure.isEmpty())
 	{
-		bool ok = false;
-		int count = curvesPerFigureList[i].toInt(&ok);
-		if(!ok)
+		QStringList curvesPerFigureList = curvesPerFigure.split(" ");
+		QStringList curvesNamesList = curveNames.split(" ");
+
+		int j=0;
+		for(int i=0; i<curvesPerFigureList.size(); ++i)
 		{
-			QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
-			break;
-		}
-		else
-		{
-			_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
-			for(int k=1; k<count && j<curveNames.size(); ++k)
+			bool ok = false;
+			int count = curvesPerFigureList[i].toInt(&ok);
+			if(!ok)
 			{
-				_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
+				QMessageBox::warning(this, "Loading failed", "Corrupted figures setup...");
+				break;
+			}
+			else
+			{
+				_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
+				for(int k=1; k<count && j<curveNames.size(); ++k)
+				{
+					_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
+				}
 			}
 		}
 	}

guilib/src/PreferencesDialog.cpp

@@ -471,6 +471,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_activateRGBD->setObjectName(Parameters::kRGBDEnabled().c_str());
 	_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
+	_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
 	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
 	_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
@@ -489,7 +490,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
+	_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str());
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str());
 
@@ -2041,16 +2042,15 @@ void PreferencesDialog::selectSourceDatabase(bool user)
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
-	QFile file(path);
-	if(!path.isEmpty() && file.exists())
+	QStringList paths = QFileDialog::getOpenFileNames(this, tr("Select file"), dir, tr("RTAB-Map database files (*.db)"));
+	if(paths.size())
 	{
 		int r = QMessageBox::question(this, tr("Odometry in database..."), tr("Use odometry saved in database (if some saved)?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
 
 		_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_database_lineEdit_path->setText(paths.size()==1?paths.front():paths.join(";"));
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 		_ui->source_checkBox_useDbStamps->setChecked(false);
 	}

guilib/src/StatsToolBox.cpp

@@ -313,6 +313,7 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 			{
 				ULOGGER_WARN("Already added to the figure");
 			}
+			emit figuresSetupChanged();
 		}
 		else
 		{
@@ -362,6 +363,7 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
 			delete curve;
 		}
 		figure->show();
+		emit figuresSetupChanged();
 
 		emit menuChanged(_plotMenu);
 	}
@@ -386,6 +388,7 @@ void StatsToolBox::figureDeleted(QObject * obj)
 					break;
 				}
 			}
+			emit figuresSetupChanged();
 		}
 		else
 		{

guilib/src/StatsToolBox.h

@@ -94,6 +94,7 @@ public slots:
 
 signals:
 	void menuChanged(const QMenu *);
+	void figuresSetupChanged();
 
 private slots:
 	void plot(const StatItem * stat, const QString & plotName = QString());

guilib/src/ui/DatabaseViewer.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1183</width>
-    <height>834</height>
+    <height>804</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -50,8 +50,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>159</width>
-             <height>136</height>
+             <width>162</width>
+             <height>150</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -97,14 +97,14 @@
               </property>
              </widget>
             </item>
-            <item row="4" column="0">
+            <item row="5" column="0">
              <widget class="QLabel" name="label_childrenA_6">
               <property name="text">
                <string>Stamp</string>
               </property>
              </widget>
             </item>
-            <item row="4" column="1">
+            <item row="5" column="1">
              <widget class="QLabel" name="label_stampA">
               <property name="text">
                <string/>
@@ -125,6 +125,20 @@
               </property>
              </widget>
             </item>
+            <item row="4" column="0">
+             <widget class="QLabel" name="label_childrenA_10">
+              <property name="text">
+               <string>Map ID</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QLabel" name="label_mapA">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
            </layout>
           </widget>
          </widget>
@@ -205,8 +219,8 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>159</width>
-             <height>136</height>
+             <width>162</width>
+             <height>150</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -252,14 +266,14 @@
               </property>
              </widget>
             </item>
-            <item row="4" column="0">
+            <item row="5" column="0">
              <widget class="QLabel" name="label_childrenA_7">
               <property name="text">
                <string>Stamp</string>
               </property>
              </widget>
             </item>
-            <item row="4" column="1">
+            <item row="5" column="1">
              <widget class="QLabel" name="label_stampB">
               <property name="text">
                <string/>
@@ -280,6 +294,20 @@
               </property>
              </widget>
             </item>
+            <item row="4" column="0">
+             <widget class="QLabel" name="label_childrenA_11">
+              <property name="text">
+               <string>Map ID</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QLabel" name="label_mapB">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
            </layout>
           </widget>
          </widget>
@@ -356,7 +384,7 @@
      <x>0</x>
      <y>0</y>
      <width>1183</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -679,7 +707,7 @@
     <layout class="QVBoxLayout" name="verticalLayout_10">
      <item>
       <layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
-       <item row="4" column="1">
+       <item row="5" column="1">
         <widget class="QLabel" name="label_15">
          <property name="text">
           <string>Iteration</string>
@@ -714,21 +742,21 @@
          </property>
         </widget>
        </item>
-       <item row="5" column="1">
+       <item row="6" column="1">
         <widget class="QLabel" name="label_11">
          <property name="text">
           <string>Point to plane</string>
          </property>
         </widget>
        </item>
-       <item row="6" column="1">
+       <item row="7" column="1">
         <widget class="QLabel" name="label_20">
          <property name="text">
           <string>Normal K neighbors</string>
          </property>
         </widget>
        </item>
-       <item row="7" column="1">
+       <item row="8" column="1">
         <widget class="QLabel" name="label_27">
          <property name="text">
           <string>2D icp (laser scans required)</string>
@@ -799,7 +827,7 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
+       <item row="5" column="0">
         <widget class="QSpinBox" name="spinBox_icp_iteration">
          <property name="minimum">
           <number>1</number>
@@ -812,7 +840,7 @@
          </property>
         </widget>
        </item>
-       <item row="6" column="0">
+       <item row="7" column="0">
         <widget class="QSpinBox" name="spinBox_icp_normalKSearch">
          <property name="minimum">
           <number>1</number>
@@ -825,7 +853,7 @@
          </property>
         </widget>
        </item>
-       <item row="5" column="0">
+       <item row="6" column="0">
         <widget class="QCheckBox" name="checkBox_icp_p2plane">
          <property name="text">
           <string/>
@@ -835,7 +863,7 @@
          </property>
         </widget>
        </item>
-       <item row="7" column="0">
+       <item row="8" column="0">
         <widget class="QCheckBox" name="checkBox_icp_2d">
          <property name="text">
           <string/>
@@ -845,6 +873,32 @@
          </property>
         </widget>
        </item>
+       <item row="4" column="1">
+        <widget class="QLabel" name="label_46">
+         <property name="text">
+          <string>Min correspondence ratio</string>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="0">
+        <widget class="QDoubleSpinBox" name="doubleSpinBox_icp_minCorrespondenceRatio">
+         <property name="suffix">
+          <string/>
+         </property>
+         <property name="decimals">
+          <number>2</number>
+         </property>
+         <property name="minimum">
+          <double>0.000000000000000</double>
+         </property>
+         <property name="maximum">
+          <double>1.000000000000000</double>
+         </property>
+         <property name="value">
+          <double>0.000000000000000</double>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>

guilib/src/ui/mainWindow.ui

@@ -195,13 +195,6 @@
      </property>
      <addaction name="separator"/>
     </widget>
-    <widget class="QMenu" name="menuFigures">
-     <property name="title">
-      <string>Figures</string>
-     </property>
-     <addaction name="actionSave_state"/>
-     <addaction name="actionLoad_state"/>
-    </widget>
     <widget class="QMenu" name="menuAspect_ratio_2">
      <property name="title">
       <string>Aspect ratio</string>
@@ -217,7 +210,6 @@
      <addaction name="action240p"/>
     </widget>
     <addaction name="menuShow_view"/>
-    <addaction name="menuFigures"/>
     <addaction name="actionScreenshot"/>
     <addaction name="actionAuto_screen_capture"/>
     <addaction name="menuAspect_ratio_2"/>
@@ -878,16 +870,6 @@
     <string>Clear the cache</string>
    </property>
   </action>
-  <action name="actionSave_state">
-   <property name="text">
-    <string>Save state</string>
-   </property>
-  </action>
-  <action name="actionLoad_state">
-   <property name="text">
-    <string>Load state</string>
-   </property>
-  </action>
   <action name="actionAuto_screen_capture">
    <property name="checkable">
     <bool>true</bool>

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>760</width>
-            <height>1029</height>
+            <height>1542</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4972,7 +4972,7 @@ 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="QDoubleSpinBox" name="rgdb_newMapOdomChange">
                          <property name="suffix">
                           <string> m</string>
@@ -4991,7 +4991,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="3" column="1">
                         <widget class="QLabel" name="label_163">
                          <property name="text">
                           <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
@@ -5001,14 +5001,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
+                       <item row="4" column="0">
                         <widget class="QCheckBox" name="odomScanHistory">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="4" column="1">
                         <widget class="QLabel" name="label_scanMatching">
                          <property name="text">
                           <string>Pose scan matching: odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
@@ -5018,7 +5018,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="6" column="1">
                         <widget class="QLabel" name="label_scanMatching_3">
                          <property name="text">
                           <string>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).</string>
@@ -5028,10 +5028,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
+                       <item row="6" column="0">
                         <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
                        </item>
-                       <item row="6" column="0">
+                       <item row="7" column="0">
                         <widget class="QDoubleSpinBox" name="localDetection_radius">
                          <property name="suffix">
                           <string> m</string>
@@ -5041,7 +5041,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="7" column="1">
                         <widget class="QLabel" name="label_space2">
                          <property name="text">
                           <string>Local radius for nodes selection in the local map. This parameter is used in some approaches below.</string>
@@ -5051,6 +5051,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="0">
+                        <widget class="QCheckBox" name="rgdb_rehearsalWeightIgnoredWhileMoving">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_165">
+                         <property name="text">
+                          <string>Ignore weight update when the robot is moving (over linear and angular update values above).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5207,7 +5224,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QGroupBox" name="groupBox_localDetection_space">
                       <property name="title">
-                       <string>Local loop closure detection in space (laser scans are required)</string>
+                       <string>Local loop closure detection in space</string>
                       </property>
                       <property name="checkable">
                        <bool>true</bool>
@@ -5216,7 +5233,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item>
                         <widget class="QLabel" name="label_space1_2">
                          <property name="text">
-                          <string>Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used. The laser scans are compared to those inside a radius around the current estimated position of the robot. If the laser scans match, a constraint is added to the graph.  Useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string>
+                          <string>Activate local detection over locations (in Working Memory) near in space. The locations are compared to those inside the local radius of the current estimated position of the robot (at a maximum distance of the path filtering radius below). If the laser scans are used, local locations are merged together and the current laser scan is compared to them. It is useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5254,7 +5271,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="0" 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>
+                            <string>Maximum graph depth between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -5281,7 +5298,7 @@ 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_3">
                            <property name="text">
-                            <string>Path filtering radius to avoid merging laser scans which are close.</string>
+                            <string>Path filtering radius to avoid merging laser scans which are close. 0 to ignore.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -5291,7 +5308,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="2" column="1">
                           <widget class="QLabel" name="label_scanMatching_4">
                            <property name="text">
-                            <string>When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.</string>
+                            <string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.8.11</version>
+  <version>0.8.12</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>