GraphViewer: added Show/Hide global path

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 10)
+SET(RTABMAP_PATCH_VERSION 12)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/CameraRGBD.h

@@ -278,6 +278,7 @@ public:
 	enum Type{
 		kTypeRGBDepthSD,
 		kTypeRGBDepthHD,
+		kTypeIRDepth,
 		kTypeRGBIR
 	};
 

corelib/include/rtabmap/core/DBDriver.h

@@ -95,7 +95,7 @@ public:
 
 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
+	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const;
 	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -134,7 +134,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
+	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;

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,8 +66,8 @@ protected:
 	virtual void mainLoop();
 
 private:
-	std::string _path;
-	float _frameRate;
+	std::list<std::string> _paths;
+	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 
@@ -70,6 +75,7 @@ private:
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	double _previousStamp;
 };
 
 } /* namespace rtabmap */

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);
 
@@ -154,7 +160,7 @@ public:
 
 	// RGB-D stuff
 	void getMetricConstraints(
-			const std::vector<int> & ids,
+			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
@@ -165,8 +171,8 @@ public:
 	bool getBowForce2D() const {return _bowForce2D;}
 	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
 	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
@@ -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, false, "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.");
@@ -289,16 +290,17 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
-	RTABMAP_PARAM(RGBD, PlanVirtualLinksMaxDiffID,  int, 50, "Max difference ID to add virtual links before planning.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
+	RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
 
 	// 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, 50,      "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
 	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -373,10 +375,10 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
 
-	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, 	"Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.05, 			"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, 		"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.025, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity
 	RTABMAP_PARAM(Stereo, WinSize,               int, 16,        "See cv::calcOpticalFlowPyrLK().");

corelib/include/rtabmap/core/Rtabmap.h

@@ -149,8 +149,13 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	std::map<int, Transform> optimizeGraph(
+			int fromId,
+			const std::set<int> & ids,
+			bool lookInDatabase,
+			std::multimap<int, Link> * constraints = 0) const;
 	void updateGoalIndex();
-	bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
+	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -179,8 +184,10 @@ private:
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localRadius;
-	int _localDetectMaxDiffID;
+	float _localImmunizationRatio;
+	int _localDetectMaxGraphDepth;
 	float _localPathFilteringRadius;
+	bool _localPathOdomPosesUsed;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	bool _reextractLoopClosureFeatures;
@@ -191,7 +198,6 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planVirtualLinks;
-	int _planVirtualLinksMaxDiffID;
 	bool _goalsSavedInUserData;
 
 	std::pair<int, float> _loopClosureHypothesis;
@@ -220,6 +226,7 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+	Transform _lastLocalizationPose; // for localization mode
 
 	// Planning stuff
 	std::vector<std::pair<int,Transform> > _path;

corelib/include/rtabmap/core/SensorData.h

@@ -62,6 +62,7 @@ public:
 
 	// Metric constructor + 2d laser scan
 	SensorData(const cv::Mat & laserScan,
+			int laserScanMaxPts,
 		  const cv::Mat & image,
 		  const cv::Mat & depthOrRightImage,
 		  float fx,
@@ -95,6 +96,7 @@ public:
 	cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
 	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
 	const cv::Mat & laserScan() const {return _laserScan;}
+	int laserScanMaxPts() const {return _laserScanMaxPts;}
 	float fx() const {return _fx;}
 	float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
 	float cx() const {return _cx;}
@@ -133,6 +135,7 @@ private:
 	Transform _localTransform;
 	float _poseRotVariance;
 	float _poseTransVariance;
+	int _laserScanMaxPts;
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;

corelib/include/rtabmap/core/Signature.h

@@ -68,7 +68,8 @@ public:
 			float fy = 0.0f,
 			float cx = 0.0f,
 			float cy = 0.0f,
-			const Transform & localTransform =Transform::getIdentity());
+			const Transform & localTransform =Transform::getIdentity(),
+			int laserScanMaxPts = 0);
 	virtual ~Signature();
 
 	/**
@@ -128,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) {_laserScanCompressed = bytes;}
+	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;}
@@ -143,11 +144,13 @@ public:
 	float getCx() const {return _cx;}
 	float getCy() const {return _cy;}
 	const Transform & getPose() const {return _pose;}
+	void getPoseVariance(float & rotVariance, float & transVariance) const;
 	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) {_laserScanRaw = depth2D;}
+	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
 	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
+	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 
 	SensorData toSensorData();
 	void uncompressData();
@@ -183,6 +186,7 @@ private:
 	Transform _pose;
 	Transform _localTransform; // camera_link -> base_link
 	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
+	int _laserScanMaxPts;
 
 	cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
 	cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1

corelib/include/rtabmap/core/Statistics.h

@@ -63,20 +63,28 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, VisualInliers,);
 	RTABMAP_STATS(Loop, Last_id,);
 
-	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	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,);
+	RTABMAP_STATS(OdomCorrection, Inliers,);
+	RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
+	RTABMAP_STATS(OdomCorrection, Variance,);
+
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
 	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Immunized_globally,);
+	RTABMAP_STATS(Memory, Immunized_locally,);
+	RTABMAP_STATS(Memory, Immunized_locally_max,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	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);
@@ -143,6 +151,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;}
@@ -165,6 +174,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;}
 
@@ -195,6 +205,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/include/rtabmap/core/util3d.h

@@ -121,6 +121,12 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
+float RTABMAP_EXP getDepth(
+		const cv::Mat & depthImage,
+		float x, float y,
+		bool smoothing,
+		float maxZError = 0.02f);
+
 pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,
@@ -286,7 +292,7 @@ Transform RTABMAP_EXP icp(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -295,7 +301,7 @@ Transform RTABMAP_EXP icpPointToPlane(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 Transform RTABMAP_EXP icp2D(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -304,7 +310,7 @@ Transform RTABMAP_EXP icp2D(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/CameraRGBD.cpp

@@ -94,10 +94,9 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
 {
 	bool warnFrameRateTooHigh = false;
 	float actualFrameRate = 0;
-	float imageRate = _imageRate==0.0f?20.0f:_imageRate; // limit to 20Hz if infinity
-	if(imageRate>0)
+	if(_imageRate>0)
 	{
-		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
+		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
 		if(sleepTime > 2)
 		{
 			uSleep(sleepTime-2);
@@ -109,14 +108,14 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
 		}
 
 		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
 		{
 			//
 		}
 
 		double slept = _frameRateTimer->getElapsedTime();
 		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
 	}
 
 	UTimer timer;
@@ -143,7 +142,7 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
 	if(warnFrameRateTooHigh)
 	{
 		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-				imageRate, actualFrameRate, timer.ticks());
+				_imageRate, actualFrameRate, timer.ticks());
 	}
 	else
 	{
@@ -367,7 +366,7 @@ bool CameraOpenNICV::init(const std::string & calibrationFolder)
 
 bool CameraOpenNICV::isCalibrated() const
 {
-	return _depthFocal > 0.0f;
+	return true;
 }
 
 void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
@@ -697,7 +696,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder)
 
 bool CameraOpenNI2::isCalibrated() const
 {
-	return _depthFx > 0.0f && _depthFy > 0.0f;
+	return true;
 }
 
 std::string CameraOpenNI2::getSerial() const
@@ -1048,10 +1047,7 @@ bool CameraFreenect::init(const std::string & calibrationFolder)
 
 bool CameraFreenect::isCalibrated() const
 {
-#ifdef WITH_FREENECT
-	return freenectDevice_ != 0 && freenectDevice_->getDepthFocal() > 0.0f;
-#endif
-	return false;
+	return true;
 }
 
 std::string CameraFreenect::getSerial() const
@@ -1129,6 +1125,9 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
 	case kTypeRGBIR:
 		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
 		break;
+	case kTypeIRDepth:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
+		break;
 	case kTypeRGBDepthSD:
 	case kTypeRGBDepthHD:
 	default:
@@ -1321,10 +1320,14 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 
 			switch(type_)
 			{
-			case kTypeRGBIR:
+			case kTypeRGBIR: //used for calibration
 				rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
 				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
 				break;
+			case kTypeIRDepth:
+				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
+				depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
+				break;
 			case kTypeRGBDepthSD:
 			case kTypeRGBDepthHD:
 			default:
@@ -1333,128 +1336,171 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 				break;
 			}
 
-			cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
-			cv::flip(rgbMat, rgb, 1);
-
-			if(stereoModel_.isValid())
+			if(irFrame && depthFrame)
 			{
-				//rectify color
-				rgb = stereoModel_.right().rectifyImage(rgb);
-				if(irFrame)
+				cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data);
+				//convert to gray scaled
+				float maxIr_ = 0x7FFF;
+				float minIr_ = 0x0;
+				const float factor = 255.0f / float((maxIr_ - minIr_));
+				rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
+				for(int i=0; i<irMat.rows; ++i)
 				{
-					//rectify IR
-					cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
-					cv::flip(depth, depth, 1);
+					for(int j=0; j<irMat.cols; ++j)
+					{
+						rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
+					}
+				}
+
+				cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+				cv::flip(rgb, rgb, 1);
+				cv::flip(depth, depth, 1);
+				if(stereoModel_.isValid())
+				{
+					//rectify
+					rgb = stereoModel_.left().rectifyImage(rgb);
 					depth = stereoModel_.left().rectifyImage(depth);
+					fx = stereoModel_.left().fx();
+					fy = stereoModel_.left().fy();
+					cx = stereoModel_.left().cx();
+					cy = stereoModel_.left().cy();
 				}
 				else
 				{
-					//rectify depth
-					cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
-					cv::flip(depth, depth, 1);
-					depth = stereoModel_.left().rectifyDepth(depth);
-
-					bool registered = true;
-					if(registered)
-					{
-						depth = util3d::registerDepth(
-								depth,
-								stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
-								stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
-								stereoModel_.transform());
-						util3d::fillRegisteredDepthHoles(depth, true, false);
-						fx = stereoModel_.right().fx();
-						fy = stereoModel_.right().fy();
-						cx = stereoModel_.right().cx();
-						cy = stereoModel_.right().cy();
-					}
-					else
-					{
-						fx = stereoModel_.left().fx();
-						fy = stereoModel_.left().fy();
-						cx = stereoModel_.left().cx();
-						cy = stereoModel_.left().cy();
-					}
+					libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+					fx = params.fx;
+					fy = params.fy;
+					cx = params.cx;
+					cy = params.cy;
 				}
 			}
 			else
 			{
-				//use data from libfreenect2
-				if(irFrame)
+				//rgb + ir or rgb + depth
+
+				cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
+				cv::flip(rgbMat, rgb, 1);
+
+				if(stereoModel_.isValid())
 				{
-					cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+					//rectify color
+					rgb = stereoModel_.right().rectifyImage(rgb);
+					if(irFrame)
+					{
+						//rectify IR
+						cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+						depth = stereoModel_.left().rectifyImage(depth);
+					}
+					else
+					{
+						//rectify depth
+						cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+						depth = stereoModel_.left().rectifyDepth(depth);
+
+						bool registered = true;
+						if(registered)
+						{
+							depth = util3d::registerDepth(
+									depth,
+									stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
+									stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
+									stereoModel_.transform());
+							util3d::fillRegisteredDepthHoles(depth, true, false);
+							fx = stereoModel_.right().fx();
+							fy = stereoModel_.right().fy();
+							cx = stereoModel_.right().cx();
+							cy = stereoModel_.right().cy();
+						}
+						else
+						{
+							fx = stereoModel_.left().fx();
+							fy = stereoModel_.left().fy();
+							cx = stereoModel_.left().cx();
+							cy = stereoModel_.left().cy();
+						}
+					}
 				}
 				else
 				{
-					cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
-
-					//registration of the depth
-					if(reg_)
+					//use data from libfreenect2
+					if(irFrame)
 					{
-						if(type_ == kTypeRGBDepthSD)
+						cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+					}
+					else
+					{
+						cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+
+						//registration of the depth
+						if(reg_)
 						{
-							cv::Mat tmp;
-							cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
-							rgb = tmp;
-						}
-						cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
-						depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
-						for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
-						{
-							for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
+							if(type_ == kTypeRGBDepthSD)
 							{
-								float dz = depthFrameMat.at<float>(dy,dx);
-								float dz1 = depthFrameMat.at<float>(dy,dx+1);
-								float dz2 = depthFrameMat.at<float>(dy+1,dx);
-								float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
-								if(dz && dz1 && dz2 && dz3)
+								cv::Mat tmp;
+								cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
+								rgb = tmp;
+							}
+							cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
+							depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
+							for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
+							{
+								for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
 								{
-									float avg = (dz + dz1 + dz2 + dz3) / 4;
-									float thres = 0.01 * avg;
-									if( fabs(dz - avg) < thres &&
-										fabs(dz1 - avg) < thres &&
-										fabs(dz2 - avg) < thres &&
-										fabs(dz3 - avg) < thres)
+									float dz = depthFrameMat.at<float>(dy,dx);
+									float dz1 = depthFrameMat.at<float>(dy,dx+1);
+									float dz2 = depthFrameMat.at<float>(dy+1,dx);
+									float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
+									if(dz && dz1 && dz2 && dz3)
 									{
-										float cx=-1,cy=-1;
-										reg_->apply(dx, dy, dz, cx, cy);
-										if(type_==kTypeRGBDepthSD)
+										float avg = (dz + dz1 + dz2 + dz3) / 4;
+										float thres = 0.01 * avg;
+										if( fabs(dz - avg) < thres &&
+											fabs(dz1 - avg) < thres &&
+											fabs(dz2 - avg) < thres &&
+											fabs(dz3 - avg) < thres)
 										{
-											cx/=2.0f;
-											cy/=2.0f;
-										}
-										int rcx = cvRound(cx);
-										int rcy = cvRound(cy);
-										if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
-										{
-											unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
-											if(zReg == 0 || zReg > (unsigned short)dz)
+											float cx=-1,cy=-1;
+											reg_->apply(dx, dy, dz, cx, cy);
+											if(type_==kTypeRGBDepthSD)
 											{
-												zReg = (unsigned short)dz;
+												cx/=2.0f;
+												cy/=2.0f;
+											}
+											int rcx = cvRound(cx);
+											int rcy = cvRound(cy);
+											if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
+											{
+												unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
+												if(zReg == 0 || zReg > (unsigned short)dz)
+												{
+													zReg = (unsigned short)dz;
+												}
 											}
 										}
 									}
 								}
 							}
+							util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
+							util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
+							libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+							fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
+						}
+						else
+						{
+							libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+							fx = params.fx;
+							fy = params.fy;
+							cx = params.cx;
+							cy = params.cy;
 						}
-						util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
-						util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
-						libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
-						fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
-					}
-					else
-					{
-						libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
-						fx = params.fx;
-						fy = params.fy;
-						cx = params.cx;
-						cy = params.cy;
 					}
+					cv::flip(depth, depth, 1);
 				}
-				cv::flip(depth, depth, 1);
 			}
 			listener_->release(frames);
 		}

corelib/src/DBDriver.cpp

@@ -419,7 +419,8 @@ void DBDriver::getNodeData(
 		float & fy,
 		float & cx,
 		float & cy,
-		Transform & localTransform) const
+		Transform & localTransform,
+		int & laserScanMaxPts) const
 {
 	bool found = false;
 	// look in the trash
@@ -437,6 +438,7 @@ void DBDriver::getNodeData(
 			cx = s->getCx();
 			cy = s->getCy();
 			localTransform = s->getLocalTransform();
+			laserScanMaxPts = s->getLaserScanMaxPts();
 			found = true;
 		}
 	}
@@ -445,7 +447,7 @@ void DBDriver::getNodeData(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
+		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
 		_dbSafeAccessMutex.unlock();
 	}
 }

corelib/src/DBDriverSqlite3.cpp

@@ -458,7 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			if(uStrNumCmp(_version, "0.7.0") >= 0)
+			if(uStrNumCmp(_version, "0.8.11") >= 0)
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
 			{
 				query << "SELECT Image.data, "
 						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -553,14 +563,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					}
 					(*iter)->setLocalTransform(localTransform);
 
+					int laserScanMaxPts = 0;
+					if(uStrNumCmp(_version, "0.8.11") >= 0)
+					{
+						laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
+					}
+
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 					//Create the laserScan
 					if(dataSize>4 && data)
 					{
-						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
+						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
 					}
-
 				}
 
 				rc = sqlite3_step(ppStmt); // next result...
@@ -588,7 +603,8 @@ void DBDriverSqlite3::getNodeDataQuery(
 		float & fy,
 		float & cx,
 		float & cy,
-		Transform & localTransform) const
+		Transform & localTransform,
+		int & laserScanMaxPts) const
 {
 	if(_ppDb)
 	{
@@ -598,7 +614,17 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.7.0") >= 0)
+		if(uStrNumCmp(_version, "0.8.11") >= 0)
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.7.0") >= 0)
 		{
 			query << "SELECT Image.data, "
 					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
@@ -675,6 +701,12 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(localTransform.data(), data, dataSize);
 			}
 
+			laserScanMaxPts = 0;
+			if(uStrNumCmp(_version, "0.8.11") >= 0)
+			{
+				laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
+			}
+
 			data = sqlite3_column_blob(ppStmt, index); // depth2d
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
 			//Create the depth2d
@@ -1255,7 +1287,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			if(visualWords.size()==0)
 			{
-				UINFO("Empty signature detected! (id=%d)", (*iter)->id());
+				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
 			}
 			else
 			{
@@ -2044,7 +2076,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			//metric
 			if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform());
+				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
 			}
 		}
 		// Finalize (delete) the statement
@@ -2222,7 +2254,11 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
-	if(uStrNumCmp(_version, "0.7.0") >= 0)
+	if(uStrNumCmp(_version, "0.8.11") >= 0)
+	{
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.7.0") >= 0)
 	{
 		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
 	}
@@ -2239,7 +2275,8 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		float fy,
 		float cx,
 		float cy,
-		const Transform & localTransform) const
+		const Transform & localTransform,
+		int depth2dMaxPts) const
 {
 	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
 	if(!ppStmt)
@@ -2293,6 +2330,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.8.11") >= 0)
+	{
+		rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

@@ -80,7 +80,8 @@ private:
 			float & fy,
 			float & cx,
 			float & cy,
-			Transform & localTransform) const;
+			Transform & localTransform,
+			int & laserScanMaxPts) const;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
@@ -110,7 +111,8 @@ private:
 			float fy,
 			float cx,
 			float cy,
-			const Transform & localTransform) const;
+			const Transform & localTransform,
+			int depth2dMaxPts) const;
 	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;

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()
@@ -75,10 +87,18 @@ bool DBReader::init(int startIndex)
 	}
 	_ids.clear();
 	_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;
 	}
 
@@ -90,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;
@@ -118,10 +138,7 @@ bool DBReader::init(int startIndex)
 
 void DBReader::setFrameRate(float frameRate)
 {
-	if(frameRate >= 0.0f)
-	{
-		_frameRate = frameRate;
-	}
+	_frameRate = frameRate;
 }
 
 void DBReader::mainLoopBegin()
@@ -199,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());
+		}
+
 	}
 
 }
@@ -210,26 +242,6 @@ SensorData DBReader::getNextData()
 	SensorData data;
 	if(_dbDriver)
 	{
-		float frameRate = _frameRate;
-		if(frameRate>0.0f)
-		{
-			int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
-			if(sleepTime > 2)
-			{
-				uSleep(sleepTime-2);
-			}
-
-			// Add precision at the cost of a small overhead
-			while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
-			{
-				//
-			}
-
-			double slept = _timer.getElapsedTime();
-			_timer.start();
-			UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
-		}
-
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
 			cv::Mat imageBytes;
@@ -241,7 +253,8 @@ SensorData DBReader::getNextData()
 			float rotVariance = 1.0f;
 			float transVariance = 1.0f;
 			std::vector<unsigned char> userData;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
+			int laserScanMaxPts = 0;
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
 
 			// info
 			int weight;
@@ -272,32 +285,83 @@ SensorData DBReader::getNextData()
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
 
-			rtabmap::CompressionThread ctImage(imageBytes, true);
-			rtabmap::CompressionThread ctDepth(depthBytes, true);
-			rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
-			ctImage.start();
-			ctDepth.start();
-			ctLaserScan.start();
-			ctImage.join();
-			ctDepth.join();
-			ctLaserScan.join();
-			data = SensorData(
-					ctLaserScan.getUncompressedData(),
-					ctImage.getUncompressedData(),
-					ctDepth.getUncompressedData(),
-					fx,fy,cx,cy,
-					localTransform,
-					pose,
-					rotVariance,
-					transVariance,
-					seq,
-					stamp,
-					userData);
-			UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
-					data.laserScan().empty()?0:1,
-					data.image().empty()?0:1,
-					data.depth().empty()?0:1,
-					data.rightImage().empty()?0:1);
+			// Frame rate
+			if(_frameRate < 0.0f)
+			{
+				if(stamp == 0)
+				{
+					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
+					this->kill();
+				}
+				else if(_previousStamp > 0)
+				{
+					int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
+					if(sleepTime > 2)
+					{
+						uSleep(sleepTime-2);
+					}
+
+					// Add precision at the cost of a small overhead
+					while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
+					{
+						//
+					}
+
+					double slept = _timer.getElapsedTime();
+					_timer.start();
+					UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
+				}
+				_previousStamp = stamp;
+			}
+			else if(_frameRate>0.0f)
+			{
+				int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
+				if(sleepTime > 2)
+				{
+					uSleep(sleepTime-2);
+				}
+
+				// Add precision at the cost of a small overhead
+				while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
+				{
+					//
+				}
+
+				double slept = _timer.getElapsedTime();
+				_timer.start();
+				UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
+			}
+
+			if(!this->isKilled())
+			{
+				rtabmap::CompressionThread ctImage(imageBytes, true);
+				rtabmap::CompressionThread ctDepth(depthBytes, true);
+				rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
+				ctImage.start();
+				ctDepth.start();
+				ctLaserScan.start();
+				ctImage.join();
+				ctDepth.join();
+				ctLaserScan.join();
+				data = SensorData(
+						ctLaserScan.getUncompressedData(),
+						laserScanMaxPts,
+						ctImage.getUncompressedData(),
+						ctDepth.getUncompressedData(),
+						fx,fy,cx,cy,
+						localTransform,
+						pose,
+						rotVariance,
+						transVariance,
+						seq,
+						stamp,
+						userData);
+				UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
+						data.laserScan().empty()?0:1,
+						data.image().empty()?0:1,
+						data.depth().empty()?0:1,
+						data.rightImage().empty()?0:1);
+			}
 		}
 	}
 	else

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

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Compression.h"
+#include "rtabmap/core/Graph.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
@@ -73,6 +74,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
+	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -328,6 +330,8 @@ Memory::~Memory()
 	UDEBUG("");
 	if(!_memoryChanged && !_linksChanged)
 	{
+		if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
+
 		UDEBUG("");
 		if(_dbDriver)
 		{
@@ -399,6 +403,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 +842,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 +866,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 +933,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 +1329,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 +1963,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 +2070,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;
@@ -2006,7 +2102,8 @@ Transform Memory::computeIcpTransform(
 		bool icp3D,
 		std::string * rejectedMsg,
 		int * inliers,
-		double * variance)
+		double * variance,
+		float * inliersRatio)
 {
 	Signature * oldS = this->_getSignature(oldId);
 	Signature * newS = this->_getSignature(newId);
@@ -2064,7 +2161,7 @@ Transform Memory::computeIcpTransform(
 			newS->uncompressData(0, 0, &tmp2);
 		}
 
-		t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance);
+		t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg, inliers, variance, inliersRatio);
 	}
 	else
 	{
@@ -2085,8 +2182,9 @@ Transform Memory::computeIcpTransform(
 		Transform guess,
 		bool icp3D,
 		std::string * rejectedMsg,
-		int * inliers,
-		double * variance) const
+		int * correspondencesOut,
+		double * varianceOut,
+		float * correspondencesRatioOut) const
 {
 	if(guess.isNull())
 	{
@@ -2145,6 +2243,7 @@ Transform Memory::computeIcpTransform(
 					Transform icpT;
 					int correspondences = 0;
 					float correspondencesRatio = -1.0f;
+					double variance = 1;
 					if(_icpPointToPlane)
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ, _icpPointToPlaneNormalNeighbors);
@@ -2161,7 +2260,7 @@ Transform Memory::computeIcpTransform(
 								   _icpMaxCorrespondenceDistance,
 								   _icpMaxIterations,
 								   &hasConverged,
-								   variance,
+								   &variance,
 								   &correspondences);
 						}
 					}
@@ -2172,23 +2271,31 @@ Transform Memory::computeIcpTransform(
 								_icpMaxCorrespondenceDistance,
 								_icpMaxIterations,
 								&hasConverged,
-								variance,
+								&variance,
 								&correspondences);
 					}
 
 					// verify if there are enough correspondences
-					correspondencesRatio = float(correspondences)/float(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size());
+					correspondencesRatio = float(correspondences)/float(newS.getDepthRaw().total());
 
 					UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
 							hasConverged?"true":"false",
-							variance?*variance:-1,
+							variance,
 							correspondences,
 							(int)(oldCloudXYZ->size()>newCloudXYZ->size()?oldCloudXYZ->size():newCloudXYZ->size()),
 							correspondencesRatio*100.0f);
 
-					if(inliers)
+					if(varianceOut)
 					{
-						*inliers = correspondences;
+						*varianceOut = variance;
+					}
+					if(correspondencesOut)
+					{
+						*correspondencesOut = correspondences;
+					}
+					if(correspondencesRatioOut)
+					{
+						*correspondencesRatioOut = correspondencesRatio;
 					}
 
 					if(!icpT.isNull() && hasConverged &&
@@ -2217,8 +2324,8 @@ Transform Memory::computeIcpTransform(
 					}
 					else
 					{
-						msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
-								hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icpCorrespondenceRatio);
+						msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)",
+								hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio);
 						UINFO(msg.c_str());
 					}
 				}
@@ -2267,21 +2374,31 @@ Transform Memory::computeIcpTransform(
 				bool hasConverged = false;
 				float correspondencesRatio = -1.0f;
 				int correspondences = 0;
+				double variance = 1;
 				icpT = util3d::icp2D(newCloud,
 						oldCloud,
 					   _icp2MaxCorrespondenceDistance,
 					   _icp2MaxIterations,
 					   &hasConverged,
-					   variance,
+					   &variance,
 					   &correspondences);
 
 				// verify if there are enough correspondences
-				correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
+
+				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("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
 						newS.id(), oldS.id(),
 						hasConverged?"true":"false",
-						variance?*variance:-1,
+						variance,
 						correspondences,
 						(int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()),
 						correspondencesRatio*100.0f);
@@ -2296,12 +2413,22 @@ Transform Memory::computeIcpTransform(
 				//	UWARN("saved newCloudFinal.pcd");
 				//}
 
-				if(inliers)
+				if(varianceOut)
 				{
-					*inliers = correspondences;
+					*varianceOut = variance;
+				}
+				if(correspondencesOut)
+				{
+					*correspondencesOut = correspondences;
+				}
+				if(correspondencesRatioOut)
+				{
+					*correspondencesRatioOut = correspondencesRatio;
 				}
 
-				if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
+				if(!icpT.isNull() &&
+					hasConverged &&
+					correspondencesRatio >= _icp2CorrespondenceRatio)
 				{
 					float ix,iy,iz, iroll,ipitch,iyaw;
 					icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
@@ -2326,8 +2453,8 @@ Transform Memory::computeIcpTransform(
 				}
 				else
 				{
-					msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
-							hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icp2CorrespondenceRatio);
+					msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)",
+							hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
 					UINFO(msg.c_str());
 				}
 			}
@@ -2362,6 +2489,9 @@ Transform Memory::computeScanMatchingTransform(
 		int * inliers,
 		double * variance)
 {
+	UASSERT(uContains(poses, newId) && uContains(_signatures, newId));
+	UASSERT(uContains(poses, oldId) && uContains(_signatures, oldId));
+
 	// make sure that all depth2D are loaded
 	std::list<Signature*> depthToLoad;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -2407,7 +2537,6 @@ Transform Memory::computeScanMatchingTransform(
 	// get the new cloud
 	Signature * newS = _getSignature(newId);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
-	UASSERT(uContains(poses, newId));
 	cv::Mat newScan;
 	newS->uncompressData(0, 0, &newScan);
 	newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
@@ -2434,9 +2563,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(),
@@ -2447,16 +2585,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);
-			//pcl::io::savePCDFile("new.pcd", *newCloud);
-			//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
-			//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
-			//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
 		}
 		else
 		{
@@ -2756,15 +2898,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 new one
+						signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
+						sB->setWeight(0);
+						UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
+								signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
+					}
 				}
 				else if(this->rehearsalMerge(id, signature->id()))
 				{
@@ -2985,7 +3135,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
 		{
@@ -3322,11 +3472,12 @@ void Memory::copyData(const Signature * from, Signature * to)
 			cv::Mat laserScan;
 			float fx, fy, cx, cy;
 			Transform localTransform;
-			_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform);
+			int laserScanMaxPts = 0;
+			_dbDriver->getNodeData(from->id(), image, depth, laserScan, fx, fy, cx, cy, localTransform, laserScanMaxPts);
 
 			to->setImageCompressed(image);
 			to->setDepthCompressed(depth, fx, fy, cx, cy);
-			to->setLaserScanCompressed(laserScan);
+			to->setLaserScanCompressed(laserScan, laserScanMaxPts);
 			to->setLocalTransform(localTransform);
 
 			UDEBUG("Loaded image data from database");
@@ -3335,7 +3486,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());
 		}
 
@@ -3872,7 +4023,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			fyOrBaseline,
 			cx,
 			cy,
-			data.localTransform());
+			data.localTransform(),
+			data.laserScanMaxPts());
 	}
 	else
 	{
@@ -3885,13 +4037,21 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			words3D,
 			data.pose(),
 			data.userData(),
-			rtabmap::compressData2(laserScan));
+			rtabmap::compressData2(laserScan),
+			cv::Mat(),
+			cv::Mat(),
+			0,
+			0,
+			0,
+			0,
+			Transform(),
+			data.laserScanMaxPts());
 	}
 	if(this->isRawDataKept())
 	{
 		s->setImageRaw(image);
 		s->setDepthRaw(depthOrRightImage);
-		s->setLaserScanRaw(laserScan);
+		s->setLaserScanRaw(laserScan, data.laserScanMaxPts());
 	}
 
 
@@ -4099,56 +4259,47 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
 	return std::set<int>(idsToLoad.begin(), idsToLoad.end());
 }
 
+// return all non-null poses
+// return unique links between nodes (for neighbors: old->new, for loops: parent->child)
 void Memory::getMetricConstraints(
-		const std::vector<int> & ids,
+		const std::set<int> & ids,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & links,
 		bool lookInDatabase)
 {
 	UDEBUG("");
-	for(unsigned int i=0; i<ids.size(); ++i)
+	for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		Transform pose = getOdomPose(ids[i], lookInDatabase);
+		Transform pose = getOdomPose(*iter, lookInDatabase);
 		if(!pose.isNull())
 		{
-			poses.insert(std::make_pair(ids[i], pose));
+			poses.insert(std::make_pair(*iter, pose));
 		}
 	}
 
-	for(unsigned int i=0; i<ids.size(); ++i)
+	for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		if(uContains(poses, ids[i]))
+		if(uContains(poses, *iter))
 		{
-			std::map<int, Link> neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors
+			std::map<int, Link> neighbors = this->getNeighborLinks(*iter, lookInDatabase); // only direct neighbors
 			for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
 			{
-				if(uContains(poses, jter->first) && jter->second.isValid())
+				if(	jter->second.isValid() &&
+					uContains(poses, jter->first) &&
+					graph::findLink(links, *iter, jter->first) == links.end())
 				{
-					bool edgeAlreadyAdded = false;
-					for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
-							iter != links.end() && iter->first == jter->first;
-							++iter)
-					{
-						if(iter->second.to() == ids[i])
-						{
-							edgeAlreadyAdded = true;
-						}
-					}
-					if(!edgeAlreadyAdded)
-					{
-						links.insert(std::make_pair(ids[i], jter->second));
-					}
+					links.insert(std::make_pair(*iter, jter->second));
 				}
 			}
 
-			std::map<int, Link> loops = this->getLoopClosureLinks(ids[i], lookInDatabase);
+			std::map<int, Link> loops = this->getLoopClosureLinks(*iter, lookInDatabase);
 			for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
 			{
-				if(jter->first < ids[i] &&
-					uContains(poses, jter->first) &&
-					jter->second.isValid()) // null transform means a child (rehearsed location)
+				if( jter->second.isValid() && // null transform means a rehearsed location
+					jter->first < *iter && // Loop parent to child
+					uContains(poses, jter->first))
 				{
-					links.insert(std::make_pair(ids[i],jter->second));
+					links.insert(std::make_pair(*iter, jter->second));
 				}
 			}
 		}

corelib/src/SensorData.cpp

@@ -45,7 +45,8 @@ SensorData::SensorData() :
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity()),
 	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f)
+	_poseTransVariance(1.0f),
+	_laserScanMaxPts(0)
 {
 }
 
@@ -63,9 +64,11 @@ SensorData::SensorData(const cv::Mat & image,
 	_localTransform(Transform::getIdentity()),
 	_poseRotVariance(1.0f),
 	_poseTransVariance(1.0f),
+	_laserScanMaxPts(0),
 	_userData(userData)
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
 }
 
@@ -95,9 +98,11 @@ SensorData::SensorData(const cv::Mat & image,
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
 	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(0),
 	_userData(userData)
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
 	UASSERT(depthOrRightImage.empty() ||
 			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
@@ -109,6 +114,7 @@ SensorData::SensorData(const cv::Mat & image,
 
 	// Metric constructor + 2d depth
 SensorData::SensorData(const cv::Mat & laserScan,
+		  int laserScanMaxPts,
 		  const cv::Mat & image,
 		  const cv::Mat & depthOrRightImage,
 		  float fx,
@@ -135,15 +141,17 @@ SensorData::SensorData(const cv::Mat & laserScan,
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
 	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(laserScanMaxPts),
 	_userData(userData)
 {
 	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+	UASSERT(depthOrRightImage.empty() ||
+			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
 			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
 			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!depthOrRightImage.empty());
 	UASSERT(!_localTransform.isNull());
 	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
 }

corelib/src/Signature.cpp

@@ -48,7 +48,8 @@ Signature::Signature() :
 	_fx(0.0f),
 	_fy(0.0f),
 	_cx(0.0f),
-	_cy(0.0f)
+	_cy(0.0f),
+	_laserScanMaxPts(0)
 {
 }
 
@@ -69,7 +70,8 @@ Signature::Signature(
 		float fy,
 		float cx,
 		float cy,
-		const Transform & localTransform) :
+		const Transform & localTransform,
+		int laserScanMaxPts) :
 	_id(id),
 	_mapId(mapId),
 	_stamp(stamp),
@@ -90,7 +92,8 @@ Signature::Signature(
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_words3(words3)
+	_words3(words3),
+	_laserScanMaxPts(laserScanMaxPts)
 {
 }
 
@@ -251,14 +254,13 @@ float Signature::getDepthFy() const {return getFy();}
 float Signature::getDepthCx() const {return getCx();}
 float Signature::getDepthCy() const {return getCy();}
 
-SensorData Signature::toSensorData()
+void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
 {
-	this->uncompressData();
-	float rotVariance = 1.0f;
-	float transVariance = 1.0f;
+	rotVariance = 1.0f;
+	transVariance = 1.0f;
 	if(_links.size())
 	{
-		for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
+		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
 		{
 			if(iter->second.kNeighbor)
 			{
@@ -272,7 +274,17 @@ SensorData Signature::toSensorData()
 			}
 		}
 	}
+}
+
+SensorData Signature::toSensorData()
+{
+	this->uncompressData();
+	float rotVariance = 1.0f;
+	float transVariance = 1.0f;
+	this->getPoseVariance(rotVariance, transVariance);
+
 	return SensorData(_laserScanRaw,
+			_laserScanMaxPts,
 			_imageRaw,
 			_depthRaw,
 			_fx,

corelib/src/Statistics.cpp

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

corelib/src/VWDictionary.cpp

@@ -209,6 +209,10 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	{
 		_incrementalDictionary = false;
 	}
+	else if(_incrementalDictionary)
+	{
+		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
+	}
 	_dictionaryPath = dictionaryPath;
 }
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -42,6 +42,7 @@ CREATE TABLE Depth (
 	cy FLOAT,
 	local_transform BLOB,
 	data2d BLOB,     -- compressed data (Laser scan)
+	data2d_max_pts INTEGER,   -- Laser scan max points
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

corelib/src/util3d.cpp

@@ -770,19 +770,15 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }
 
-pcl::PointXYZ projectDepthTo3D(
+float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
-		float cx, float cy,
-		float fx, float fy,
 		bool smoothing,
 		float maxZError)
 {
+	UASSERT(!depthImage.empty());
 	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
 
-	pcl::PointXYZ pt;
-	float bad_point = std::numeric_limits<float>::quiet_NaN ();
-
 	int u = int(x+0.5f);
 	int v = int(y+0.5f);
 
@@ -790,8 +786,7 @@ pcl::PointXYZ projectDepthTo3D(
 	{
 		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
 				x,u,y,v,depthImage.cols, depthImage.rows);
-		pt.x = pt.y = pt.z = bad_point;
-		return pt;
+		return 0;
 	}
 
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
@@ -845,7 +840,30 @@ pcl::PointXYZ projectDepthTo3D(
 			// mean
 			depth = (depth+sumDepths)/sumWeights;
 		}
+	}
+	else
+	{
+		depth = 0;
+	}
+	return depth;
+}
 
+pcl::PointXYZ projectDepthTo3D(
+		const cv::Mat & depthImage,
+		float x, float y,
+		float cx, float cy,
+		float fx, float fy,
+		bool smoothing,
+		float maxZError)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+
+	pcl::PointXYZ pt;
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	float depth = getDepth(depthImage, x, y, smoothing, maxZError);
+	if(depth)
+	{
 		// Use correct principal point from calibration
 		cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
 		cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
@@ -1933,7 +1951,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  int maximumIterations,
 			  bool * hasConvergedOut,
 			  double * variance,
-			  int * inliers)
+			  int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -1956,7 +1974,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	bool hasConverged = icp.hasConverged();
 
 	// compute variance
-	if((inliers || variance) && hasConverged)
+	if((correspondencesOut || variance) && hasConverged)
 	{
 		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -1986,16 +2004,16 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			}
 		}
 
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = (int)correspondences.size();
+			*correspondencesOut = (int)correspondences.size();
 		}
 	}
 	else
 	{
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = 0;
+			*correspondencesOut = 0;
 		}
 		if(variance)
 		{
@@ -2019,7 +2037,7 @@ Transform icpPointToPlane(
 		int maximumIterations,
 		bool * hasConvergedOut,
 		double * variance,
-		int * inliers)
+		int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
 	// Set the input source and target
@@ -2046,7 +2064,7 @@ Transform icpPointToPlane(
 	bool hasConverged = icp.hasConverged();
 
 	// compute variance
-	if((inliers || variance) && hasConverged)
+	if((correspondencesOut || variance) && hasConverged)
 	{
 		pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
@@ -2076,16 +2094,16 @@ Transform icpPointToPlane(
 			}
 		}
 
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = (int)correspondences.size();
+			*correspondencesOut = (int)correspondences.size();
 		}
 	}
 	else
 	{
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = 0;
+			*correspondencesOut = 0;
 		}
 		if(variance)
 		{
@@ -2108,7 +2126,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  int maximumIterations,
 			  bool * hasConvergedOut,
 			  double * variance,
-			  int * inliers)
+			  int * correspondencesOut)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -2135,7 +2153,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	bool hasConverged = icp.hasConverged();
 
 	// compute variance
-	if((inliers || variance) && hasConverged)
+	if((correspondencesOut || variance) && hasConverged)
 	{
 		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
@@ -2165,16 +2183,16 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			}
 		}
 
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = (int)correspondences.size();
+			*correspondencesOut = (int)correspondences.size();
 		}
 	}
 	else
 	{
-		if(inliers)
+		if(correspondencesOut)
 		{
-			*inliers = 0;
+			*correspondencesOut = 0;
 		}
 		if(variance)
 		{
@@ -2577,12 +2595,32 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	cv::Mat affineTransform(2,3,CV_32FC1);
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
+		UASSERT(!iter->second.isNull());
+
+		iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+
 		if(uContains(occupancy, iter->first))
 		{
-			UASSERT(!iter->second.isNull());
 			const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
-
-			iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 			cosT = cos(yaw);
 			sinT = sin(yaw);
 			affineTransform.at<float>(0,0) = cosT;
@@ -2592,25 +2630,6 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			affineTransform.at<float>(0,2) = x;
 			affineTransform.at<float>(1,2) = y;
 
-			if(undefinedSize)
-			{
-				minX = maxX = x;
-				minY = maxY = y;
-				undefinedSize = false;
-			}
-			else
-			{
-				if(minX > x)
-					minX = x;
-				else if(maxX < x)
-					maxX = x;
-
-				if(minY > y)
-					minY = y;
-				else if(maxY < y)
-					maxY = y;
-			}
-
 			//ground
 			if(pair.first.rows)
 			{

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -47,7 +47,7 @@ class RTABMAPGUI_EXP CalibrationDialog  : public QDialog, public UEventsHandler
 	Q_OBJECT;
 
 public:
-	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", QWidget * parent = 0);
+	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0);
 	virtual ~CalibrationDialog();
 
 	bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);}
@@ -58,6 +58,7 @@ public:
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
 
+	void setSwitchedImages(bool switched);
 	void setStereoMode(bool stereo);
 	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 

guilib/include/rtabmap/gui/CloudViewer.h

@@ -224,7 +224,7 @@ private:
     QAction * _aSetGridCellSize;
     QAction * _aSetBackgroundColor;
     QMenu * _menu;
-    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr > _graphes;
+    std::set<std::string> _graphes;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
     unsigned int _gridCellCount;

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -89,6 +89,7 @@ private slots:
 	void refineAllLoopClosureLinks();
 	void refineVisuallyAllNeighborLinks();
 	void refineVisuallyAllLoopClosureLinks();
+	void resetAllChanges();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);
@@ -120,7 +121,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 +141,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/ImageView.h

@@ -74,9 +74,9 @@ public:
 	void setGraphicsViewScaled(bool scaled);
 	void setBackgroundColor(const QColor & color);
 
-	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color = Qt::yellow);
-	void setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color = Qt::yellow);
-	void addFeature(int id, const cv::KeyPoint & kpt, QColor color);
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
+	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
+	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color);
 	void setImage(const QImage & image);
 	void setImageDepth(const QImage & image);

guilib/include/rtabmap/gui/KeypointItem.h

@@ -41,7 +41,7 @@ namespace rtabmap {
 class RTABMAPGUI_EXP KeypointItem : public QGraphicsEllipseItem
 {
 public:
-	KeypointItem(int id, const cv::KeyPoint & kpt, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
+	KeypointItem(int id, const cv::KeyPoint & kpt, float depth = 0, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
 	virtual ~KeypointItem();
 
 	void setColor(const QColor & color);
@@ -61,6 +61,7 @@ private:
 	cv::KeyPoint _kpt;
 	QGraphicsRectItem * _placeHolder;
 	int _width;
+	float _depth;
 };
 
 }

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/include/rtabmap/gui/PreferencesDialog.h

@@ -124,6 +124,7 @@ public:
 	bool imageRejectedShown() const;
 	bool imageHighestHypShown() const;
 	bool beepOnPause() const;
+	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
 
@@ -178,16 +179,18 @@ public:
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
+	bool getSourceDatabaseStampsUsed() const;//Database group
 	Src getSourceRGBD() const; 			// Openni group
 	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group
 	bool getSourceOpenni2AutoExposure() const;  //Openni group
 	int getSourceOpenni2Exposure() const;  //Openni group
 	int getSourceOpenni2Gain() const;   //Openni group
 	bool getSourceOpenni2Mirroring() const; //Openni group
+	int getSourceFreenect2Format() const; //Openni group
 	bool isSourceRGBDColorOnly() const;
 	QString getSourceOpenniDevice() const;            //Openni group
 	Transform getSourceOpenniLocalTransform() const;    //Openni group
-	CameraRGBD * createCameraRGBD() const; // return camera should be deleted if not null
+	CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 
@@ -245,7 +248,7 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
-	void updateOpenNI2GroupBoxVisibility();
+	void updateRGBDCameraGroupBoxVisibility();
 	void testOdometry();
 	void testRGBDCamera();
 

guilib/src/CalibrationDialog.cpp

@@ -46,7 +46,7 @@ namespace rtabmap {
 
 #define COUNT_MIN 40
 
-CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, QWidget * parent) :
+CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
 	QDialog(parent),
 	stereo_(stereo),
 	savingDirectory_(savingDirectory),
@@ -91,6 +91,8 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 
 	ui_->radioButton_raw->setChecked(true);
 
+	ui_->checkBox_switchImages->setChecked(switchImages);
+
 	this->setStereoMode(stereo_);
 }
 
@@ -136,6 +138,11 @@ void CalibrationDialog::loadSettings(QSettings & settings, const QString & group
 	}
 }
 
+void CalibrationDialog::setSwitchedImages(bool switched)
+{
+	ui_->checkBox_switchImages->setChecked(switched);
+}
+
 void CalibrationDialog::setStereoMode(bool stereo)
 {
 	this->restart();

guilib/src/CloudViewer.cpp

@@ -552,7 +552,7 @@ void CloudViewer::addOrUpdateGraph(
 
 	if(graph->size())
 	{
-		_graphes.insert(std::make_pair(id, graph));
+		_graphes.insert(id);
 
 		pcl::PolygonMesh mesh;
 		pcl::Vertices vertices;
@@ -565,6 +565,9 @@ void CloudViewer::addOrUpdateGraph(
 		mesh.polygons.push_back(vertices);
 		_visualizer->addPolylineFromPolygonMesh(mesh, id);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
+
+		this->addOrUpdateCloud(id+"_nodes", graph, Transform::getIdentity(), color);
+		this->setCloudPointSize(id+"_nodes", 5);
 	}
 }
 
@@ -580,16 +583,18 @@ void CloudViewer::removeGraph(const std::string & id)
 	{
 		_visualizer->removeShape(id);
 		_graphes.erase(id);
+		removeCloud(id+"_nodes");
 	}
 }
 
 void CloudViewer::removeAllGraphs()
 {
-	for(std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = _graphes.begin(); iter!=_graphes.end(); ++iter)
+	std::set<std::string> graphes = _graphes;
+	for(std::set<std::string>::iterator iter = graphes.begin(); iter!=graphes.end(); ++iter)
 	{
-		_visualizer->removeShape(iter->first);
+		this->removeGraph(*iter);
 	}
-	_graphes.clear();
+	UASSERT(_graphes.empty());
 }
 
 bool CloudViewer::isTrajectoryShown() const

guilib/src/DatabaseViewer.cpp

@@ -87,13 +87,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 
 	ui_->dockWidget_constraints->setVisible(false);
 	ui_->dockWidget_graphView->setVisible(false);
-	ui_->dockWidget_icp->setVisible(false);
-	ui_->dockWidget_visual->setVisible(false);
+	ui_->dockWidget_parameters->setVisible(false);
 	ui_->dockWidget_stereoView->setVisible(false);
 	ui_->dockWidget_view3d->setVisible(false);
-	ui_->dockWidget_detectMoreLC->setVisible(false);
-	ui_->dockWidget_graphOptimizer->setVisible(false);
-	ui_->dockWidget_gridcloud->setVisible(false);
 
 	ui_->constraintsViewer->setCameraLockZ(false);
 	ui_->constraintsViewer->setCameraFree();
@@ -124,13 +120,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 
 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_icp->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_visual->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_detectMoreLC->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_graphOptimizer->toggleViewAction());
-	ui_->menuView->addAction(ui_->dockWidget_gridcloud->toggleViewAction());
+	ui_->menuView->addAction(ui_->dockWidget_parameters->toggleViewAction());
 	connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
 
 	connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
@@ -151,6 +143,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
 	connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
+	connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
 
 	//ICP buttons
 	connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
@@ -237,6 +230,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()));
@@ -259,13 +253,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	}
 	ui_->dockWidget_constraints->installEventFilter(this);
 	ui_->dockWidget_graphView->installEventFilter(this);
-	ui_->dockWidget_icp->installEventFilter(this);
 	ui_->dockWidget_stereoView->installEventFilter(this);
-	ui_->dockWidget_visual->installEventFilter(this);
 	ui_->dockWidget_view3d->installEventFilter(this);
-	ui_->dockWidget_detectMoreLC->installEventFilter(this);
-	ui_->dockWidget_graphOptimizer->installEventFilter(this);
-	ui_->dockWidget_gridcloud->installEventFilter(this);
+	ui_->dockWidget_parameters->installEventFilter(this);
 }
 
 DatabaseViewer::~DatabaseViewer()
@@ -354,6 +344,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 +420,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
@@ -624,6 +616,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 void DatabaseViewer::showEvent(QShowEvent* anEvent)
 {
 	this->setWindowModified(false);
+
+	if(ui_->graphViewer->isVisible() && graphes_.size() && localMaps_.size()==0)
+	{
+		sliderIterationsValueChanged((int)graphes_.size()-1);
+	}
 }
 
 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
@@ -671,61 +668,97 @@ void DatabaseViewer::exportDatabase()
 		if(!dialog.outputPath().isEmpty())
 		{
 			int framesIgnored = dialog.framesIgnored();
+			double frameRate = dialog.targetFramerate();
 			int sessionExported = dialog.sessionExported();
 			QString path = dialog.outputPath();
 			rtabmap::DataRecorder recorder;
 			QList<int> ids;
-			if(sessionExported < 0)
+
+			double previousStamp = 0;
+			std::vector<double> delays(ids_.size());
+			int oi=0;
+			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
-				ids = ids_;
-			}
-			else
-			{
-				for(int i=0; i<ids_.size(); ++i)
+				Transform odomPose;
+				int weight = -1;
+				int mapId = -1;
+				std::string label;
+				double stamp = 0;
+				std::vector<unsigned char> userData;
+				if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
 				{
-					Transform odomPose;
-					int weight = -1;
-					int mapId = -1;
-					std::string label;
-					double stamp = 0;
-					std::vector<unsigned char> userData;
-					if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
+					if(frameRate == 0 ||
+					   previousStamp == 0 ||
+					   stamp == 0 ||
+					   stamp - previousStamp >= 1.0/frameRate)
 					{
-						if(sessionExported == mapId)
+						if(sessionExported < 0 || sessionExported == mapId)
 						{
 							ids.push_back(ids_[i]);
+
+							if(previousStamp && stamp)
+							{
+								delays[oi++] = stamp - previousStamp;
+							}
+							previousStamp = stamp;
 						}
-						else if(mapId > sessionExported)
-						{
-							break;
-						}
+					}
+					if(sessionExported >= 0 && mapId > sessionExported)
+					{
+						break;
 					}
 				}
 			}
+			delays.resize(oi);
+
 			if(recorder.init(path, false))
 			{
-				rtabmap::DetailedProgressDialog progressDialog(this);
-				progressDialog.setMaximumSteps(ids.size() / (1+framesIgnored) + 1);
-				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+=1+framesIgnored)
+				for(int i=0; i<ids.size(); ++i)
 				{
 					int id = ids.at(i);
 
 					Signature data = memory_->getSignatureData(id, true);
-					rtabmap::SensorData sensorData = data.toSensorData();
-					if(!dialog.isUserDataExported())
+					float rotVariance = 1.0f;
+					float transVariance = 1.0f;
+					if(dialog.isOdomExported())
 					{
-						sensorData.setUserData(std::vector<unsigned char>());
+						data.getPoseVariance(rotVariance, transVariance);
 					}
+					rtabmap::SensorData sensorData(
+						dialog.isDepth2dExported()?data.getLaserScanRaw():cv::Mat(),
+						dialog.isDepth2dExported()?data.getLaserScanMaxPts():0,
+						dialog.isRgbExported()?data.getImageRaw():cv::Mat(),
+						dialog.isDepthExported()?data.getDepthRaw():cv::Mat(),
+						dialog.isRgbExported() || dialog.isDepthExported()?data.getFx():0,
+						dialog.isRgbExported() || dialog.isDepthExported()?data.getFy():0,
+						dialog.isRgbExported() || dialog.isDepthExported()?data.getCx():0,
+						dialog.isRgbExported() || dialog.isDepthExported()?data.getCy():0,
+						dialog.isRgbExported() || dialog.isDepthExported()?data.getLocalTransform():Transform::getIdentity(),
+						dialog.isOdomExported()?data.getPose():Transform(),
+						rotVariance,
+						transVariance,
+						data.id(),
+						data.getStamp(),
+						dialog.isUserDataExported()?data.getUserData():std::vector<unsigned char>());
+
 					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
 			{
@@ -788,7 +821,7 @@ void DatabaseViewer::updateIds()
 	{
 		//get constraints only for parent links
 
-		memory_->getMetricConstraints(std::vector<int>(ids.begin(), ids.end()), poses_, links_, true);
+		memory_->getMetricConstraints(ids, poses_, links_, true);
 
 		if(poses_.size())
 		{
@@ -1258,7 +1291,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();
@@ -1283,7 +1316,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();
@@ -1308,7 +1341,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();
@@ -1333,7 +1366,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();
@@ -1346,6 +1379,15 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
 	}
 }
 
+void DatabaseViewer::resetAllChanges()
+{
+	linksAdded_.clear();
+	linksRefined_.clear();
+	linksRemoved_.clear();
+	updateLoopClosuresSlider();
+	this->updateGraphView();
+}
+
 void DatabaseViewer::sliderAValueChanged(int value)
 {
 	this->update(value,
@@ -1357,7 +1399,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)
@@ -1371,7 +1415,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,
@@ -1384,6 +1430,7 @@ void DatabaseViewer::update(int value,
 						rtabmap::ImageView * view,
 						rtabmap::CloudViewer * view3D,
 						QLabel * labelId,
+						QLabel * labelMapId,
 						bool updateConstraintView)
 {
 	UTimer timer;
@@ -1392,6 +1439,7 @@ void DatabaseViewer::update(int value,
 	labelChildren->clear();
 	weight->clear();
 	label->clear();
+	labelMapId->clear();
 	stamp->clear();
 	QRectF rect;
 	if(value >= 0 && value < ids_.size())
@@ -1419,7 +1467,7 @@ void DatabaseViewer::update(int value,
 
 				if(data.getWords().size())
 				{
-					view->setFeatures(data.getWords());
+					view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow);
 				}
 
 				Transform odomPose;
@@ -1465,6 +1513,11 @@ void DatabaseViewer::update(int value,
 								1);
 					}
 					view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform());
+
+					//add scan
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw());
+					view3D->addOrUpdateCloud("1", scan);
+
 					view3D->update();
 				}
 			}
@@ -1512,11 +1565,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
@@ -1841,14 +1890,20 @@ 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.type() == Link::kNeighbor)
+	if(ids_.size())
 	{
-		this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
-	}
-	else
-	{
-		this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+		Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
+		if(link.isValid())
+		{
+			if(link.type() == Link::kNeighbor)
+			{
+				this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
+			}
+			else
+			{
+				this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+			}
+		}
 	}
 }
 
@@ -1923,6 +1978,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,
@@ -1934,6 +1990,7 @@ void DatabaseViewer::updateConstraintView(
 					ui_->graphicsView_B,
 					ui_->widget_cloudB,
 					ui_->label_idB,
+					ui_->label_mapB,
 					false); // don't update constraints view!
 	}
 
@@ -2042,7 +2099,10 @@ void DatabaseViewer::updateConstraintView(
 					if(cloudTo->size())
 					{
 						cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudTo);
-						cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
+						if(cloudTo->size())
+						{
+							cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
+						}
 					}
 
 					if(cloudFrom->size())
@@ -2091,10 +2151,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
 		{
@@ -2102,10 +2170,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
@@ -2437,10 +2513,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)
 	{
@@ -2487,6 +2563,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
@@ -2515,6 +2592,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!");
+					}
+				}
 			}
 		}
 	}
@@ -2625,53 +2714,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)
 	{
@@ -2737,6 +2848,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;
@@ -2766,6 +2885,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()
@@ -2830,8 +2955,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 
 			if(!silent)
 			{
-				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords());
-				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords());
+				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth());
+				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth());
 				updateWordsMatching();
 			}
 		}
@@ -2846,16 +2971,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())
 			{
@@ -2876,6 +2992,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/ExportDialog.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ui_exportDialog.h"
 
 #include <QFileDialog>
+#include <QPushButton>
 
 namespace rtabmap {
 
@@ -40,7 +41,11 @@ ExportDialog::ExportDialog(QWidget * parent) :
 
 	connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
 
+	restoreDefaults();
+	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+
 	connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_framerate, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_session, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -48,7 +53,7 @@ ExportDialog::ExportDialog(QWidget * parent) :
 	connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 
-	_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
+	_ui->lineEdit_path->setText(QDir::currentPath()+QDir::separator()+"output.db");
 }
 
 ExportDialog::~ExportDialog()
@@ -56,6 +61,58 @@ ExportDialog::~ExportDialog()
 	delete _ui;
 }
 
+void ExportDialog::saveSettings(QSettings & settings, const QString & group) const
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	settings.setValue("framesIgnored", this->framesIgnored());
+	settings.setValue("targetFramerate", this->targetFramerate());
+	settings.setValue("sessionExported", this->sessionExported());
+	settings.setValue("rgbExported", this->isRgbExported());
+	settings.setValue("depthExported", this->isDepthExported());
+	settings.setValue("depth2dExported", this->isDepth2dExported());
+	settings.setValue("odomExported", this->isOdomExported());
+	settings.setValue("userDataExported", this->isUserDataExported());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::loadSettings(QSettings & settings, const QString & group)
+{
+	if(!group.isEmpty())
+	{
+		settings.beginGroup(group);
+	}
+	_ui->spinBox_ignored->setValue(settings.value("framesIgnored", this->framesIgnored()).toInt());
+	_ui->doubleSpinBox_framerate->setValue(settings.value("targetFramerate", this->targetFramerate()).toDouble());
+	_ui->spinBox_session->setValue(settings.value("sessionExported", this->sessionExported()).toInt());
+	_ui->checkBox_rgb->setChecked(settings.value("rgbExported", this->isRgbExported()).toBool());
+	_ui->checkBox_depth->setChecked(settings.value("depthExported", this->isDepthExported()).toBool());
+	_ui->checkBox_depth2d->setChecked(settings.value("depth2dExported", this->isDepth2dExported()).toBool());
+	_ui->checkBox_odom->setChecked(settings.value("odomExported", this->isOdomExported()).toBool());
+	_ui->checkBox_userData->setChecked(settings.value("userDataExported", this->isUserDataExported()).toBool());
+	if(!group.isEmpty())
+	{
+		settings.endGroup();
+	}
+}
+
+void ExportDialog::restoreDefaults()
+{
+	_ui->spinBox_ignored->setValue(0);
+	_ui->doubleSpinBox_framerate->setValue(0);
+	_ui->spinBox_session->setValue(-1);
+	_ui->checkBox_rgb->setChecked(true);
+	_ui->checkBox_depth->setChecked(true);
+	_ui->checkBox_depth2d->setChecked(true);
+	_ui->checkBox_odom->setChecked(true);
+	_ui->checkBox_userData->setChecked(false);
+}
+
 void ExportDialog::getPath()
 {
 	QString path = QFileDialog::getSaveFileName(this, tr("Output database path..."), _ui->lineEdit_path->text(), tr("RTAB-Map database (*.db)"));
@@ -75,6 +132,11 @@ int ExportDialog::framesIgnored() const
 	return _ui->spinBox_ignored->value();
 }
 
+double ExportDialog::targetFramerate() const
+{
+	return _ui->doubleSpinBox_framerate->value();
+}
+
 int ExportDialog::sessionExported() const
 {
 	return _ui->spinBox_session->value();

guilib/src/ExportDialog.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define EXPORTDIALOG_H_
 
 #include <QDialog>
+#include <QSettings>
 
 class Ui_ExportDialog;
 
@@ -43,8 +44,12 @@ public:
 
 	virtual ~ExportDialog();
 
+	void saveSettings(QSettings & settings, const QString & group) const;
+	void loadSettings(QSettings & settings, const QString & group);
+
 	QString outputPath() const;
 	int framesIgnored() const;
+	double targetFramerate() const;
 	int sessionExported() const;
 	bool isRgbExported() const;
 	bool isDepthExported() const;
@@ -57,6 +62,7 @@ signals:
 
 private slots:
 	void getPath();
+	void restoreDefaults();
 
 private:
 	Ui_ExportDialog * _ui;

guilib/src/GraphViewer.cpp

@@ -75,6 +75,7 @@ public:
 		this->setBrush(b);
 	}
 
+	const Transform & pose() const {return _pose;}
 	void setPose(const Transform & pose) {this->setPos(-pose.y(),-pose.x()); _pose=pose;}
 
 protected:
@@ -158,6 +159,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 +172,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 +206,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 +257,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 +344,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,8 +394,13 @@ 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);
+	if(_localRadius->isVisible())
+	{
+		this->ensureVisible(_localRadius, 0, 0);
+	}
 }
 
 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
@@ -456,36 +471,92 @@ 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)
 {
+	for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
 	if(localPath.size() > 1)
 	{
 		for(unsigned int i=0; i<localPath.size()-1; ++i)
 		{
-			if(_linkItems.contains(localPath[i]))
+			int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
+			int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
+			if(_nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
 			{
-				int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
-				int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
-				QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
-				while(itemIter.key() == idFrom && itemIter != _linkItems.end())
+				bool updated = false;
+				if(_localPathLinkItems.contains(idFrom))
 				{
-					if(itemIter.value()->to() == idTo)
+					QMultiMap<int, LinkItem*>::iterator itemIter = _localPathLinkItems.find(idFrom);
+					while(itemIter.key() == idFrom && itemIter != _localPathLinkItems.end())
 					{
-						itemIter.value()->setColor(_localPathColor);
-						break;
+						if(itemIter.value()->to() == idTo)
+						{
+							itemIter.value()->setPoses(_nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose());
+							itemIter.value()->show();
+							updated = true;
+							break;
+						}
+						++itemIter;
 					}
-					++itemIter;
+				}
+				if(!updated)
+				{
+					//create a link item
+					LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link::kUndef);
+					QPen p = item->pen();
+					p.setWidthF(_linkWidth);
+					item->setPen(p);
+					item->setColor(_localPathColor);
+					this->scene()->addItem(item);
+					item->setZValue(16); // just over the global path
+					item->setParentItem(_root);
+					_localPathLinkItems.insert(idFrom, item);
 				}
 			}
 		}
 	}
+
+	// remove not used links
+	for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _localPathLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
 }
 
 void GraphViewer::clearGraph()
@@ -494,9 +565,13 @@ void GraphViewer::clearGraph()
 	_nodeItems.clear();
 	qDeleteAll(_linkItems);
 	_linkItems.clear();
+	qDeleteAll(_localPathLinkItems);
+	_localPathLinkItems.clear();
 	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 +605,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 +616,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 +632,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 +643,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 +662,10 @@ bool GraphViewer::isReferentialVisible() const
 {
 	return _referential->isVisible();
 }
+bool GraphViewer::isLocalRadiusVisible() const
+{
+	return _localRadius->isVisible();
+}
 
 void GraphViewer::setWorkingDirectory(const QString & path)
 {
@@ -619,6 +702,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 +786,10 @@ void GraphViewer::setReferentialVisible(bool visible)
 {
 	_referential->setVisible(visible);
 }
+void GraphViewer::setLocalRadiusVisible(bool visible)
+{
+	_localRadius->setVisible(visible);
+}
 
 void GraphViewer::restoreDefaults()
 {
@@ -741,7 +832,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 +867,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aShowHideGridMap;
 	QAction * aShowHideOrigin;
 	QAction * aShowHideReferential;
+	QAction * aShowHideLocalRadius;
+	QAction * aClearGlobalPath;
 	if(_gridMap->isVisible())
 	{
 		aShowHideGridMap = menu.addAction(tr("Hide grid map"));
@@ -798,6 +893,23 @@ 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"));
+	}
+	if(_globalPathLinkItems.size() && _globalPathLinkItems.begin().value()->isVisible())
+	{
+		aClearGlobalPath = menu.addAction(tr("Hide global path"));
+	}
+	else
+	{
+		aClearGlobalPath = menu.addAction(tr("Show global path"));
+	}
+	aClearGlobalPath->setEnabled(_globalPathLinkItems.size());
 	menu.addSeparator();
 	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
@@ -869,6 +981,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		return; // without emitting configChanged
 	}
 	else if(r == aChangeNodeColor ||
+			r == aChangeCurrentGoalColor ||
 			r == aChangeNeighborColor ||
 			r == aChangeGlobalLoopColor ||
 			r == aChangeLocalLoopColor ||
@@ -882,6 +995,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _nodeColor;
 		}
+		else if(r == aChangeCurrentGoalColor)
+		{
+			color = _currentGoalColor;
+		}
 		else if(r == aChangeGlobalLoopColor)
 		{
 			color = _loopClosureColor;
@@ -918,6 +1035,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setNodeColor(color);
 			}
+			else if(r == aChangeCurrentGoalColor)
+			{
+				this->setCurrentGoalColor(color);
+			}
 			else if(r == aChangeGlobalLoopColor)
 			{
 				this->setGlobalLoopClosureColor(color);
@@ -978,10 +1099,21 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		this->setReferentialVisible(!this->isReferentialVisible());
 	}
+	else if(r == aShowHideLocalRadius)
+	{
+		this->setLocalRadiusVisible(!this->isLocalRadiusVisible());
+	}
 	else if(r == aRestoreDefaults)
 	{
 		this->restoreDefaults();
 	}
+	else if(r == aClearGlobalPath)
+	{
+		for(QMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
+		{
+			iter.value()->setVisible(!iter.value()->isVisible());
+		}
+	}
 	if(r)
 	{
 		emit configChanged();

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;
@@ -122,6 +129,7 @@ private:
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
+	QMultiMap<int, LinkItem*> _localPathLinkItems;
 	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;
 	float _linkWidth;
@@ -129,6 +137,7 @@ private:
 	QGraphicsItemGroup * _referential;
 	QGraphicsItemGroup * _originReferential;
 	float _gridCellSize;
+	QGraphicsEllipseItem * _localRadius;
 };
 
 } /* namespace rtabmap */

guilib/src/ImageView.cpp

@@ -38,13 +38,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QVBoxLayout>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
+#include "rtabmap/core/util3d.h"
 
 namespace rtabmap {
 
 ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
-		_alpha(100),
+		_alpha(50),
 		_imageItem(0),
 		_imageDepthItem(0)
 {
@@ -77,7 +78,7 @@ ImageView::ImageView(QWidget * parent) :
 	_graphicsViewScaled->setCheckable(true);
 	_graphicsViewScaled->setChecked(true);
 	_graphicsViewScaled->setEnabled(false);
-	_setAlpha = _menu->addAction(tr("Set alpha..."));
+	_setAlpha = _menu->addAction(tr("Set transparency..."));
 	_saveImage = _menu->addAction(tr("Save picture..."));
 	_saveImage->setEnabled(false);
 
@@ -507,7 +508,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	else if(action == _setAlpha)
 	{
 		bool ok = false;
-		int value = QInputDialog::getInt(this, tr("Set features and lines alpha"), tr("alpha (0-255)"), _alpha, 0, 255, 10, &ok);
+		int value = QInputDialog::getInt(this, tr("Set features and lines transparency"), tr("alpha (0-255)"), _alpha, 0, 255, 10, &ok);
 		if(ok)
 		{
 			this->setAlpha(value);
@@ -529,36 +530,28 @@ 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);
 	}
 }
 
-void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color)
+void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth, const QColor & color)
 {
 	qDeleteAll(_features);
 	_features.clear();
 
 	for(std::multimap<int, cv::KeyPoint>::const_iterator iter = refWords.begin(); iter != refWords.end(); ++iter )
 	{
-		addFeature(iter->first, iter->second, color);
+		addFeature(iter->first, iter->second, depth.empty()?0:util3d::getDepth(depth, iter->second.pt.x, iter->second.pt.y, false), color);
 	}
 
 	if(!_graphicsView->isVisible())
@@ -567,14 +560,14 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, c
 	}
 }
 
-void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color)
+void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth, const QColor & color)
 {
 	qDeleteAll(_features);
 	_features.clear();
 
 	for(unsigned int i = 0; i< features.size(); ++i )
 	{
-		addFeature(i, features[i], color);
+		addFeature(i, features[i], depth.empty()?0:util3d::getDepth(depth, features[i].pt.x, features[i].pt.y, false), color);
 	}
 
 	if(!_graphicsView->isVisible())
@@ -583,10 +576,10 @@ void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QC
 	}
 }
 
-void ImageView::addFeature(int id, const cv::KeyPoint & kpt, QColor color)
+void ImageView::addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color)
 {
 	color.setAlpha(this->getAlpha());
-	rtabmap::KeypointItem * item = new rtabmap::KeypointItem(id, kpt, color);
+	rtabmap::KeypointItem * item = new rtabmap::KeypointItem(id, kpt, depth, color);
 	_features.insert(id, item);
 	item->setVisible(isFeaturesShown());
 	item->setZValue(1);

guilib/src/KeypointItem.cpp

@@ -34,11 +34,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, const QColor & color, QGraphicsItem * parent) :
+KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, float depth, const QColor & color, QGraphicsItem * parent) :
 	QGraphicsEllipseItem(kpt.pt.x-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.pt.y-(kpt.size==0?3.0f:kpt.size)/2.0f, kpt.size==0?3.0f:kpt.size, kpt.size==0?3.0f:kpt.size, parent),
 	_id(id),
 	_kpt(kpt),
-	_placeHolder(0)
+	_placeHolder(0),
+	_depth(depth)
 {
 	this->setColor(color);
 	this->setAcceptHoverEvents(true);
@@ -69,12 +70,25 @@ void KeypointItem::showDescription()
 		_placeHolder->setBrush(QBrush(QColor ( 0, 0, 0, 170 ))); // Black transparent background
 		QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
 		text->setDefaultTextColor(this->pen().color().rgb());
-		text->setPlainText(QString( "Id = %1\n"
-				"Dir = %3\n"
-				"Hessian = %4\n"
-				"X = %5\n"
-				"Y = %6\n"
-				"Size = %7").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size));
+		if(_depth <= 0)
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size));
+		}
+		else
+		{
+			text->setPlainText(QString( "Id = %1\n"
+					"Dir = %3\n"
+					"Hessian = %4\n"
+					"X = %5\n"
+					"Y = %6\n"
+					"Size = %7\n"
+					"Depth = %8 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_depth));
+		}
 		_placeHolder->setRect(text->boundingRect());
 	}
 

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();
@@ -846,13 +845,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 		{
 			if(info.type == 0)
 			{
-				_ui->imageView_odometry->setFeatures(info.words, Qt::yellow);
+				_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
 			}
 			else if(info.type == 1)
 			{
 				std::vector<cv::KeyPoint> kpts;
 				cv::KeyPoint::convert(info.refCorners, kpts);
-				_ui->imageView_odometry->setFeatures(kpts, Qt::red);
+				_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
 			}
 		}
 
@@ -981,7 +980,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
-		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopOdom_corrected(), 0.0f);
+		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kOdomCorrectionAccepted(), 0.0f);
 		_ui->label_matchId->clear();
 		_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
@@ -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("");
 	}
@@ -1429,12 +1433,11 @@ void MainWindow::updateMapCloud(
 	{
 		// Find all graphs
 		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
-		pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
-		graphNodes->resize(_currentPosesMap.size());
-		int oi = 0;
 		for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
 		{
 			int mapId = uValue(_currentMapIds, iter->first, -1);
+
+			//edges
 			std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
 			if(kter == graphs.end())
 			{
@@ -1442,7 +1445,6 @@ void MainWindow::updateMapCloud(
 			}
 			pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
 			kter->second->push_back(pt);
-			(*graphNodes)[oi++] = pcl::PointXYZ(pt);
 		}
 
 		// add graphs
@@ -1451,14 +1453,10 @@ void MainWindow::updateMapCloud(
 			QColor color = Qt::gray;
 			if(iter->first >= 0)
 			{
-				color = (Qt::GlobalColor)((iter->first+2) % 12 + 7 );
+				color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
 			}
 			_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
-
-		// add nodes
-		_ui->widget_cloudViewer->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
-		_ui->widget_cloudViewer->setCloudPointSize("graph_nodes", 5);
 	}
 
 	// Update occupancy grid map in 3D map view and graph view
@@ -1475,27 +1473,13 @@ void MainWindow::updateMapCloud(
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();
-		cv::Mat map8S;
-		if(_preferencesDialog->isGridMapFrom3DCloud())
-		{
-			map8S = util3d::create2DMapFromOccupancyLocalMaps(
+		cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
 					poses,
-					_projectionLocalMaps,
+					_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
 					resolution,
 					xMin, yMin,
 					0,
 					_preferencesDialog->isGridMapEroded());
-		}
-		else if(_gridLocalMaps.size())
-		{
-			map8S = util3d::create2DMapFromOccupancyLocalMaps(
-					poses,
-					_gridLocalMaps,
-					resolution,
-					xMin, yMin,
-					0,
-					_preferencesDialog->isGridMapEroded());
-		}
 		if(!map8S.empty())
 		{
 			//convert to gray scaled map
@@ -1659,7 +1643,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			QColor color = Qt::gray;
 			if(mapId >= 0)
 			{
-				color = (Qt::GlobalColor)(mapId % 12 + 7 );
+				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 			}
 			if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
 			{
@@ -1676,7 +1660,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		QColor color = Qt::gray;
 		if(mapId >= 0)
 		{
-			color = (Qt::GlobalColor)(mapId % 12 + 7 );
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 		}
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 		cloud->resize(iter->getWords3().size());
@@ -1731,12 +1715,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		cloud = util3d::laserScanToPointCloud(depth2D);
-		QColor color = Qt::red;
+		QColor color = Qt::gray;
 		if(mapId >= 0)
 		{
-			color = (Qt::GlobalColor)(mapId % 12 + 7 );
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 		}
-		if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
+		if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
 		{
 			UERROR("Adding cloud %d to viewer failed!", nodeId);
 		}
@@ -1973,20 +1957,40 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 
 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
 {
-	if(event.getPoses().empty())
-	{
-		QMessageBox::warning(this, tr("Setting goal failed"), tr("Setting goal to location %1 failed. "
-				"Some reasons: \n"
-				"1) the location doesn't exist in the graph,\n"
-				"2) the location is not linked to the global graph or\n"
-				"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()));
-	}
-	else
+	if(!event.getPoses().empty())
 	{
 		_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);
+	}
+
+	if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
+	{
+		// use MessageBox
+		if(event.getPoses().empty())
+		{
+			QMessageBox * warn = new QMessageBox(
+					QMessageBox::Warning,
+					tr("Setting goal failed!"),
+					tr("Setting goal to location %1 failed. "
+						"Some reasons: \n"
+						"1) the location doesn't exist in the graph,\n"
+						"2) the location is not linked to the global graph or\n"
+						"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()),
+					QMessageBox::Ok,
+					this);
+			warn->setAttribute(Qt::WA_DeleteOnClose, true);
+			warn->show();
+		}
+		else
+		{
+			QMessageBox * info = new QMessageBox(
+					QMessageBox::Information,
+					tr("Goal detected!"),
+					tr("Global path computed from %1 to %2 (%3 poses, %4 m).").arg(event.getPoses().front().first).arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
+					QMessageBox::Ok,
+					this);
+			info->setAttribute(Qt::WA_DeleteOnClose, true);
+			info->show();
+		}
 	}
 }
 
@@ -2035,7 +2039,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		}
 		if(_dbReader)
 		{
-			_dbReader->setFrameRate(_preferencesDialog->getGeneralInputRate());
+			_dbReader->setFrameRate( _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate());
 		}
 	}//This will update the statistics toolbox
 
@@ -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
@@ -2169,7 +2187,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 			// GREEN = NEW
 			color = Qt::green;
 		}
-		_ui->imageView_source->addFeature(iter->first, iter->second, color);
+		_ui->imageView_source->addFeature(iter->first, iter->second, 0, color);
 	}
 	ULOGGER_DEBUG("source time = %f s", timer.ticks());
 
@@ -2207,7 +2225,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 			// GREEN = NEW
 			color = Qt::green;
 		}
-		_ui->imageView_loopClosure->addFeature(iter->first, iter->second, color);
+		_ui->imageView_loopClosure->addFeature(iter->first, iter->second, 0, color);
 	}
 
 	ULOGGER_DEBUG("loop closure time = %f s", timer.ticks());
@@ -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()
@@ -2735,7 +2756,7 @@ void MainWindow::startDetection()
 	else if(_preferencesDialog->isSourceDatabaseUsed())
 	{
 		_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
-								 _preferencesDialog->getGeneralInputRate(),
+								 _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
 								 _preferencesDialog->getSourceDatabaseOdometryIgnored(),
 								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored());
 
@@ -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);
+				}
 			}
 		}
 	}
@@ -4028,27 +4038,13 @@ void MainWindow::exportGridMap()
 
 	// create the map
 	float xMin=0.0f, yMin=0.0f;
-	cv::Mat pixels;
-	if(_preferencesDialog->isGridMapFrom3DCloud())
-	{
-		pixels = util3d::create2DMapFromOccupancyLocalMaps(
+	cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
 				poses,
-				_projectionLocalMaps,
+				_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
 				gridCellSize,
 				xMin, yMin,
 				0,
 				_preferencesDialog->isGridMapEroded());
-	}
-	else
-	{
-		pixels = util3d::create2DMapFromOccupancyLocalMaps(
-				poses,
-				_gridLocalMaps,
-				gridCellSize,
-				xMin, yMin,
-				0,
-				_preferencesDialog->isGridMapEroded());
-	}
 
 	if(!pixels.empty())
 	{

guilib/src/OdometryViewer.cpp

@@ -270,13 +270,13 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 	{
 		if(info.type == 0)
 		{
-			imageView_->setFeatures(info.words, Qt::yellow);
+			imageView_->setFeatures(info.words, data.depth(), Qt::yellow);
 		}
 		else if(info.type == 1)
 		{
 			std::vector<cv::KeyPoint> kpts;
 			cv::KeyPoint::convert(info.refCorners, kpts);
-			imageView_->setFeatures(kpts, Qt::red);
+			imageView_->setFeatures(kpts, data.depth(), Qt::red);
 		}
 
 		imageView_->clearLines();

guilib/src/PreferencesDialog.cpp

@@ -167,7 +167,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
 	// Default Driver
-	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOpenNI2GroupBoxVisibility()));
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
 	this->resetSettings(_ui->groupBox_source0);
 
 	_ui->predictionPlot->showLegend(false);
@@ -192,6 +192,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_imageRejectedShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_imageHighestHypShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_beep, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkBox_notifyWhenNewGlobalPathIsReceived, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
@@ -299,6 +300,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 	//openni group
 	connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -307,6 +310,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_freenect2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -469,6 +473,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());
@@ -481,14 +486,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
-	_ui->graphPlan_maxDiffID->setObjectName(Parameters::kRGBDPlanVirtualLinksMaxDiffID().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
-	_ui->localDetection_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());
+	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 
 	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
@@ -738,7 +744,7 @@ void PreferencesDialog::setupSignals()
 		if(obj)
 		{
 			// set tooltip as the parameter name
-			obj->setToolTip(iter->first.c_str());
+			obj->setToolTip(tr("%1 (Default=\"%2\")").arg(iter->first.c_str()).arg(iter->second.c_str()));
 
 			QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
 			QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
@@ -877,6 +883,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 	{
 		_ui->general_checkBox_imagesKept->setChecked(true);
 		_ui->checkBox_beep->setChecked(false);
+		_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(false);
 		_ui->checkBox_verticalLayoutUsed->setChecked(true);
 		_ui->checkBox_imageRejectedShown->setChecked(true);
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
@@ -940,6 +947,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
 		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
+		_ui->source_checkBox_useDbStamps->setChecked(false);
 
 		_ui->groupBox_sourceOpenni->setChecked(true);
 #ifdef _WIN32
@@ -963,6 +971,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->openni2_exposure->setValue(0);
 		_ui->openni2_gain->setValue(100);
 		_ui->openni2_mirroring->setChecked(false);
+		_ui->comboBox_freenect2Format->setCurrentIndex(0);
 		_ui->checkbox_rgbd_colorOnly->setChecked(false);
 		_ui->lineEdit_openniDevice->setText("");
 		_ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
@@ -1138,6 +1147,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_imageRejectedShown->setChecked(settings.value("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked()).toBool());
 	_ui->checkBox_imageHighestHypShown->setChecked(settings.value("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked()).toBool());
 	_ui->checkBox_beep->setChecked(settings.value("beep", _ui->checkBox_beep->isChecked()).toBool());
+	_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(settings.value("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked()).toBool());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 
@@ -1215,6 +1225,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
 	_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
 	_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
+	_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
 	settings.endGroup(); // Database
 
 	settings.beginGroup("Openni");
@@ -1225,6 +1236,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->openni2_exposure->setValue(settings.value("openni2Exposure", _ui->openni2_exposure->value()).toInt());
 	_ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt());
 	_ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool());
+	_ui->comboBox_freenect2Format->setCurrentIndex(settings.value("freenect2Format", _ui->comboBox_freenect2Format->currentIndex()).toInt());
 	_ui->checkbox_rgbd_colorOnly->setChecked(settings.value("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked()).toBool());
 	_ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString());
 	_ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString());
@@ -1406,6 +1418,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked());
 	settings.setValue("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked());
 	settings.setValue("beep", _ui->checkBox_beep->isChecked());
+	settings.setValue("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked());
 	settings.setValue("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked());
 
@@ -1482,6 +1495,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked());
 	settings.setValue("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked());
 	settings.setValue("startPos",       _ui->source_spinBox_databaseStartPos->value());
+	settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
 	settings.endGroup();
 
 	settings.beginGroup("Openni");
@@ -1492,6 +1506,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("openni2Exposure", 		_ui->openni2_exposure->value());
 	settings.setValue("openni2Gain", 			_ui->openni2_gain->value());
 	settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked());
+	settings.setValue("freenect2Format", _ui->comboBox_freenect2Format->currentIndex());
 	settings.setValue("rgbdColorOnly", _ui->checkbox_rgbd_colorOnly->isChecked());
 	settings.setValue("device", 		_ui->lineEdit_openniDevice->text());
 	settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text());
@@ -2035,17 +2050,17 @@ 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);
 	}
 
 	if(_ui->groupBox_sourceDatabase->isChecked())
@@ -2818,9 +2833,10 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
-void PreferencesDialog::updateOpenNI2GroupBoxVisibility()
+void PreferencesDialog::updateRGBDCameraGroupBoxVisibility()
 {
 	_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);
+	_ui->groupBox_freenect2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcOpenNI_PCL);
 }
 
 /*** GETTERS ***/
@@ -2861,6 +2877,10 @@ bool PreferencesDialog::beepOnPause() const
 {
 	return _ui->checkBox_beep->isChecked();
 }
+bool PreferencesDialog::notifyWhenNewGlobalPathIsReceived() const
+{
+	return _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked();
+}
 int PreferencesDialog::getOdomQualityWarnThr() const
 {
 	return _ui->spinBox_odomQualityWarnThr->value();
@@ -3060,6 +3080,10 @@ int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();
 }
+bool PreferencesDialog::getSourceDatabaseStampsUsed() const
+{
+	return _ui->source_checkBox_useDbStamps->isChecked();
+}
 
 PreferencesDialog::Src PreferencesDialog::getSourceRGBD() const
 {
@@ -3085,6 +3109,11 @@ bool PreferencesDialog::getSourceOpenni2Mirroring() const
 {
 	return _ui->openni2_mirroring->isChecked();
 }
+int PreferencesDialog::getSourceFreenect2Format() const
+{
+	return _ui->comboBox_freenect2Format->currentIndex();
+}
+
 bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
@@ -3125,42 +3154,82 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const
 	return t;
 }
 
-CameraRGBD * PreferencesDialog::createCameraRGBD() const
+CameraRGBD * PreferencesDialog::createCameraRGBD(bool forCalibration)
 {
 	if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_PCL)
 	{
-		return new CameraOpenni(
-				this->getSourceOpenniDevice().toStdString(),
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
+		if(forCalibration)
+		{
+			QMessageBox::warning(this, tr("Calibration"),
+					tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. "
+					    "Factory calibration loaded from OpenNI is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			return new CameraOpenni(
+					this->getSourceOpenniDevice().toStdString(),
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2)
 	{
-		return new CameraOpenNI2(
-				this->getSourceOpenniDevice().toStdString(),
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
+		if(forCalibration)
+	{
+		QMessageBox::warning(this, tr("Calibration"),
+				tr("RTAB-Map calibration for \"OpenNI2\" driver is not yet supported. "
+					"Factory calibration loaded from OpenNI2 is used."), QMessageBox::Ok);
+		return 0;
+	}
+		else
+		{
+			return new CameraOpenNI2(
+					this->getSourceOpenniDevice().toStdString(),
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == PreferencesDialog::kSrcFreenect)
 	{
-		return new CameraFreenect(
-				this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
+		if(forCalibration)
+		{
+			QMessageBox::warning(this, tr("Calibration"),
+					tr("RTAB-Map calibration for \"Freenect\" driver is not yet supported. "
+						"Factory calibration loaded from Freenect is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			return new CameraFreenect(
+					this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV ||
 			this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS)
 	{
-		return new CameraOpenNICV(
-				this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
-				this->getGeneralInputRate(),
-				this->getSourceOpenniLocalTransform());
+		if(forCalibration)
+		{
+			QMessageBox::warning(this, tr("Calibration"),
+					tr("RTAB-Map calibration for \"OpenNI\" driver is not yet supported. "
+						"Factory calibration loaded from OpenNI is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			return new CameraOpenNICV(
+					this->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS,
+					this->getGeneralInputRate(),
+					this->getSourceOpenniLocalTransform());
+		}
 	}
 	else if(this->getSourceRGBD() == kSrcFreenect2)
 	{
 		return new CameraFreenect2(
 			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
-			CameraFreenect2::kTypeRGBDepthSD,
+			forCalibration?CameraFreenect2::kTypeRGBIR:(CameraFreenect2::Type)getSourceFreenect2Format(),
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform());
 	}
@@ -3410,12 +3479,17 @@ void PreferencesDialog::testRGBDCamera()
 
 void PreferencesDialog::calibrate()
 {
-	CameraRGBD * camera = this->createCameraRGBD();
+	CameraRGBD * camera = this->createCameraRGBD(true);
 	if(camera == 0 || !camera->init("")) // don't set calibration folder to use raw images
 	{
-		QMessageBox::warning(this,
-			   tr("RTAB-Map"),
-			   tr("RGBD camera initialization failed!"));
+		if(camera != 0)
+		{
+			QMessageBox::warning(this,
+				   tr("RTAB-Map"),
+				   tr("RGBD camera initialization failed!"));
+		}
+		//else already warned
+
 		if(camera)
 		{
 			delete camera;
@@ -3448,7 +3522,8 @@ void PreferencesDialog::calibrate()
 			}
 		}
 	}
-	_calibrationDialog->setStereoMode(dynamic_cast<CameraStereoDC1394*>(camera)!=0);
+	_calibrationDialog->setStereoMode(true);
+	_calibrationDialog->setSwitchedImages(dynamic_cast<CameraFreenect2*>(camera) != 0);
 	_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 	_calibrationDialog->registerToEventsManager();
 

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/exportDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>382</width>
-    <height>291</height>
+    <height>331</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -45,6 +45,40 @@
      </item>
     </layout>
    </item>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_4">
+     <item>
+      <widget class="QDoubleSpinBox" name="doubleSpinBox_framerate">
+       <property name="suffix">
+        <string> Hz</string>
+       </property>
+       <property name="decimals">
+        <number>1</number>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_3">
+       <property name="text">
+        <string>Target frame rate</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_3">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+    </layout>
+   </item>
    <item>
     <layout class="QHBoxLayout" name="horizontalLayout_3">
      <item>
@@ -172,7 +206,7 @@
       <enum>Qt::Horizontal</enum>
      </property>
      <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok|QDialogButtonBox::RestoreDefaults</set>
      </property>
     </widget>
    </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

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-806</y>
+            <y>-591</y>
             <width>760</width>
-            <height>1462</height>
+            <height>1502</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -135,6 +135,26 @@
                         </property>
                        </widget>
                       </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_102">
+                        <property name="text">
+                         <string>Notify when a new global path is received.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -1603,6 +1623,20 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <bool>false</bool>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_9">
+                       <item row="0" column="2">
+                        <widget class="QToolButton" name="toolButton_dbViewer">
+                         <property name="toolTip">
+                          <string>Open database viewer</string>
+                         </property>
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="icon">
+                          <iconset resource="../GuiLib.qrc">
+                           <normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="1" column="0">
                         <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
                          <property name="text">
@@ -1620,14 +1654,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item row="0" column="1">
                         <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                        </item>
-                       <item row="3" column="1">
+                       <item row="4" column="1">
                         <widget class="QLabel" name="label_58">
                          <property name="text">
                           <string>Start position (index)</string>
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
+                       <item row="4" column="0">
                         <widget class="QSpinBox" name="source_spinBox_databaseStartPos">
                          <property name="minimum">
                           <number>0</number>
@@ -1637,20 +1671,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </property>
                         </widget>
                        </item>
-                       <item row="0" column="2">
-                        <widget class="QToolButton" name="toolButton_dbViewer">
-                         <property name="toolTip">
-                          <string>Open database viewer</string>
-                         </property>
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="icon">
-                          <iconset resource="../GuiLib.qrc">
-                           <normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="1">
                         <widget class="QLabel" name="label_72">
                          <property name="text">
@@ -1661,7 +1681,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="3" column="1">
                         <widget class="QLabel" name="label_80">
                          <property name="text">
                           <string>Ignore goal delay.</string>
@@ -1671,13 +1691,30 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="0">
+                       <item row="3" column="0">
                         <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_90">
+                         <property name="text">
+                          <string>Use database stamps as input rate.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QCheckBox" name="source_checkBox_useDbStamps">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -1906,6 +1943,47 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                          </layout>
                         </widget>
                        </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_freenect2">
+                         <property name="title">
+                          <string>Freenect2</string>
+                         </property>
+                         <layout class="QGridLayout" name="gridLayout_56" columnstretch="0,1">
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_230">
+                            <property name="text">
+                             <string>Format.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="0">
+                           <widget class="QComboBox" name="comboBox_freenect2Format">
+                            <property name="sizeAdjustPolicy">
+                             <enum>QComboBox::AdjustToContents</enum>
+                            </property>
+                            <item>
+                             <property name="text">
+                              <string>RGB+Depth SD</string>
+                             </property>
+                            </item>
+                            <item>
+                             <property name="text">
+                              <string>RGB+Depth HD</string>
+                             </property>
+                            </item>
+                            <item>
+                             <property name="text">
+                              <string>IR+Depth</string>
+                             </property>
+                            </item>
+                           </widget>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
                        <item>
                         <layout class="QFormLayout" name="formLayout">
                          <property name="fieldGrowthPolicy">
@@ -2976,6 +3054,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <string>Rehearsal / Weight Update</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_12" columnstretch="0,1">
+                       <item row="2" column="0">
+                        <widget class="QCheckBox" name="general_checkBox_RehearsalIdUpdatedToNewOne">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="1">
                         <widget class="QLabel" name="label_similarity">
                          <property name="text">
@@ -3006,8 +3091,18 @@ 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="general_checkBox_RehearsalIdUpdatedToNewOne">
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_rehearsalIdUpdate_2">
+                         <property name="text">
+                          <string>Ignore Weight Update when the robot is moving.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QCheckBox" name="rgdb_rehearsalWeightIgnoredWhileMoving">
                          <property name="text">
                           <string/>
                          </property>
@@ -5014,7 +5109,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="5" 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>
@@ -5024,7 +5119,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>
@@ -5034,6 +5129,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="6" column="1">
+                        <widget class="QLabel" name="label_scanMatching_5">
+                         <property name="text">
+                          <string>Ratio of working memory for which local nodes are immunized from transfer.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5190,7 +5314,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>
@@ -5199,7 +5323,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>
@@ -5237,21 +5361,21 @@ 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>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QLabel" name="label_scanMatching_2">
                            <property name="text">
                             <string>If local space links are kept in WM.</string>
@@ -5264,13 +5388,30 @@ 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>
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_scanMatching_4">
+                           <property name="text">
+                            <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>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5331,7 +5472,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="2" column="1">
                           <widget class="QLabel" name="label_space3_5">
                            <property name="text">
                             <string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -5341,33 +5482,13 @@ 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="2" column="0">
                           <widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_space3_6">
-                           <property name="text">
-                            <string>Maximum ID difference between close nodes to add virtual links. Set 0 to ignore.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="0">
-                          <widget class="QSpinBox" name="graphPlan_maxDiffID">
-                           <property name="maximum">
-                            <number>999</number>
-                           </property>
-                           <property name="value">
-                            <number>50</number>
-                           </property>
-                          </widget>
-                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5900,6 +6021,45 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                        <string>ICP 3D (e.g. Kinect depth)</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
+                       <item row="5" column="0">
+                        <widget class="QSpinBox" name="loopClosure_icpIterations">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>1000</number>
+                         </property>
+                         <property name="value">
+                          <number>10</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_124">
+                         <property name="text">
+                          <string>Max iterations.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="loopClosure_icpSamples">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>100</number>
+                         </property>
+                         <property name="value">
+                          <number>5000</number>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="loopClosure_icpDecimation">
                          <property name="minimum">
@@ -5910,6 +6070,22 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="4" column="0">
+                        <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="1">
                         <widget class="QLabel" name="label_109">
                          <property name="text">
@@ -5972,22 +6148,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
-                        <widget class="QSpinBox" name="loopClosure_icpSamples">
-                         <property name="minimum">
-                          <number>0</number>
-                         </property>
-                         <property name="maximum">
-                          <number>999999</number>
-                         </property>
-                         <property name="singleStep">
-                          <number>100</number>
-                         </property>
-                         <property name="value">
-                          <number>5000</number>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="3" column="1">
                         <widget class="QLabel" name="label_118">
                          <property name="text">
@@ -5998,19 +6158,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
-                        <widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
-                         <property name="minimum">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.100000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="4" column="1">
                         <widget class="QLabel" name="label_122">
                          <property name="text">
@@ -6021,29 +6168,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
-                        <widget class="QSpinBox" name="loopClosure_icpIterations">
-                         <property name="minimum">
-                          <number>1</number>
-                         </property>
-                         <property name="maximum">
-                          <number>1000</number>
-                         </property>
-                         <property name="value">
-                          <number>10</number>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="1">
-                        <widget class="QLabel" name="label_124">
-                         <property name="text">
-                          <string>Max iterations.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="6" column="0">
                         <widget class="QDoubleSpinBox" name="loopClosure_icpRatio">
                          <property name="minimum">
@@ -6121,6 +6245,9 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       <layout class="QGridLayout" name="gridLayout_52" columnstretch="0,1">
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="loopClosure_icp2MaxCorrespondenceDistance">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
                          <property name="minimum">
                           <double>0.010000000000000</double>
                          </property>
@@ -6184,7 +6311,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                        <item row="2" column="1">
                         <widget class="QLabel" name="label_148">
                          <property name="text">
-                          <string>Ratio of matching correspondences to accept the transform.</string>
+                          <string>Minimum ratio of correspondences on laser scan maximum size to accept the transform.</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.10</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>

tools/Calibration/main.cpp

@@ -128,6 +128,8 @@ int main(int argc, char * argv[])
 	UINFO("Using device %d", device);
 	UINFO("Stereo: %s", stereo?"true":"false");
 
+	bool switchImages = false;
+
 	rtabmap::Camera * cameraUsb = 0;
 	rtabmap::CameraRGBD * camera = 0;
 	if(driver == -1)
@@ -181,6 +183,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
+		switchImages = true;
 		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBIR);
 	}
 	else if(driver == 6)
@@ -230,7 +233,7 @@ int main(int argc, char * argv[])
 	}
 
 	QApplication app(argc, argv);
-	rtabmap::CalibrationDialog dialog(stereo);
+	rtabmap::CalibrationDialog dialog(stereo, ".", switchImages);
 	dialog.registerToEventsManager();
 
 	dialog.show();

tools/EpipolarGeometry/main.cpp

@@ -126,7 +126,7 @@ private:
 				// GREEN = NEW
 				color = Qt::green;
 			}
-			view1_->addFeature(id, i->second, color);
+			view1_->addFeature(id, i->second, 0, color);
 		}
 		ULOGGER_DEBUG("source time = %f s", timer.ticks());