Updated version to 0.8.11. Added param RGBD/LocalLoopDetectionPathOdomPosesUsed. DatabaseViewer: export option at a specified framerate. DBReader: option to read database at a rate specified by the stamps saved. Database: Added new column "data2d_max_pts" in Depth table

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

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

@@ -62,7 +62,7 @@ protected:
 
 private:
 	std::string _path;
-	float _frameRate;
+	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 
@@ -70,6 +70,7 @@ private:
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	double _previousStamp;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Memory.h

@@ -165,8 +165,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(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 * inliers = 0, double * variance = 0) const;
+	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,

corelib/include/rtabmap/core/Parameters.h

@@ -299,6 +299,7 @@ class RTABMAP_EXP Parameters
 	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, 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 +374,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, CorrespondenceRatio, float, 0.3, 		"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, 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,6 +149,11 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	std::map<int, Transform> optimizeGraph(
+			int fromId,
+			const std::vector<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);
 
@@ -181,6 +186,7 @@ private:
 	float _localRadius;
 	int _localDetectMaxDiffID;
 	float _localPathFilteringRadius;
+	bool _localPathOdomPosesUsed;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	bool _reextractLoopClosureFeatures;

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 = 0) {_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;}
@@ -146,8 +147,9 @@ public:
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
 	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
+	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts = 0) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
 	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
+	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 
 	SensorData toSensorData();
 	void uncompressData();
@@ -183,6 +185,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,13 +63,17 @@ 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_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,);

corelib/include/rtabmap/core/util3d.h

@@ -286,7 +286,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 +295,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 +304,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/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

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

@@ -2006,7 +2006,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 +2065,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 +2086,9 @@ Transform Memory::computeIcpTransform(
 		Transform guess,
 		bool icp3D,
 		std::string * rejectedMsg,
-		int * inliers,
+		int * correspondencesOut,
-		double * variance) const
+		double * varianceOut,
+		float * correspondencesRatioOut) const
 {
 	if(guess.isNull())
 	{
@@ -2145,6 +2147,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 +2164,7 @@ Transform Memory::computeIcpTransform(
 								   _icpMaxCorrespondenceDistance,
 								   _icpMaxIterations,
 								   &hasConverged,
-								   variance,
+								   &variance,
 								   &correspondences);
 						}
 					}
@@ -2172,23 +2175,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 +2228,8 @@ Transform Memory::computeIcpTransform(
 					}
 					else
 					{
-						msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
+						msg = uFormat("Cannot compute transform (converged=%s var=%f corr=%d corrRatio=%f/%f)",
-								hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icpCorrespondenceRatio);
+								hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icpCorrespondenceRatio);
 						UINFO(msg.c_str());
 					}
 				}
@@ -2267,21 +2278,30 @@ 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
+				{
+					correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
+				}
 
 				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 +2316,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 +2356,8 @@ Transform Memory::computeIcpTransform(
 				}
 				else
 				{
-					msg = uFormat("Cannot compute transform (converged=%s var=%f corrRatio=%f/%f)",
+					msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)",
-							hasConverged?"true":"false", variance?*variance:-1, correspondencesRatio, _icp2CorrespondenceRatio);
+							hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio);
 					UINFO(msg.c_str());
 				}
 			}
@@ -2362,6 +2392,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 +2440,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));
@@ -2452,10 +2484,10 @@ Transform Memory::computeScanMatchingTransform(
 		{
 			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
 
-			//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
+			//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
-			//pcl::io::savePCDFile("new.pcd", *newCloud);
+			//pcl::io::savePCDFile("new.pcd", *newCloud, true);
 			//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
-			//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
+			//pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
 			//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
 		}
 		else
@@ -3322,11 +3354,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");
@@ -3872,7 +3905,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			fyOrBaseline,
 			cx,
 			cy,
-			data.localTransform());
+			data.localTransform(),
+			data.laserScanMaxPts());
 	}
 	else
 	{
@@ -3885,7 +3919,15 @@ 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())
 	{

corelib/src/Rtabmap.cpp

@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
 	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
 	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
+	_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -386,6 +387,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -951,7 +953,9 @@ bool Rtabmap::process(const SensorData & data)
 			std::string rejectedMsg;
 			Transform guess = signature->getLinks().begin()->second.transform();
 			double variance = -1.0;
-			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
+			int inliers = 0;
+			float inliersRatio = 0;
+			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
 			if(!t.isNull())
 			{
 				scanMatchingSuccess = true;
@@ -966,6 +970,10 @@ bool Rtabmap::process(const SensorData & data)
 			{
 				UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
 			}
+			statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0);
+			statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
+			statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
+			statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
 		}
 		timeScanMatching = timer.ticks();
 		ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1554,17 +1562,6 @@ bool Rtabmap::process(const SensorData & data)
 					if(_localPathFilteringRadius <= 0.0f ||
 					   _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
 					{
-						// path filtering
-						if(_localPathFilteringRadius > 0.0f)
-						{
-							std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
-							// make sure the nearest and farthest poses are still here
-							filteredPath.insert(*_optimizedPoses.find(nearestId));
-							filteredPath.insert(*path.begin());
-							filteredPath.insert(*path.rbegin());
-							path = filteredPath;
-						}
-
 						// 1) look for loop closures based on visual correspondences
 						double variance = 1.0;
 						Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
@@ -1576,9 +1573,31 @@ bool Rtabmap::process(const SensorData & data)
 						}
 						if(transform.isNull())
 						{
+							// 2) Assemble scans in the path and do ICP only
+							if(_localPathOdomPosesUsed)
+							{
+								//optimize the path's poses locally
+								path = optimizeGraph(nearestId, uKeys(path), false);
+								// transform local poses in optimized graph referential
+								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
+								{
+									jter->second = t * jter->second;
+								}
+							}
+							if(_localPathFilteringRadius > 0.0f)
+							{
+								// path filtering
+								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
+								// make sure the nearest and farthest poses are still here
+								filteredPath.insert(*path.find(nearestId));
+								filteredPath.insert(*path.begin());
+								filteredPath.insert(*path.rbegin());
+								path = filteredPath;
+							}
+
 							if(path.size() > 2) // more than current+nearest
 							{
-								// 2) Assemble scans in the path and do ICP only
 								// add current node to poses
 								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
 								//The nearest will be the reference for a loop closure transform
@@ -1742,7 +1761,6 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 
-			statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
@@ -2295,36 +2313,50 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		UINFO("get ids time %f s", timer.ticks());
 
-		std::map<int, Transform> poses;
+		optimizedPoses = Rtabmap::optimizeGraph(id, uKeys(ids), lookInDatabase, constraints);
-		std::multimap<int, Link> edgeConstraints;
-		_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
-		UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
-
-		if(constraints)
-		{
-			*constraints = edgeConstraints;
-		}
-
-		UASSERT(_graphOptimizer!=0);
-		if(_graphOptimizer->iterations() == 0)
-		{
-			// Optimization desactivated! Return not optimized poses.
-			optimizedPoses = poses;
-		}
-		else
-		{
-			optimizedPoses = _graphOptimizer->optimize(id, poses, edgeConstraints);
-		}
-		UINFO("optimize time %f s", timer.ticks());
 
 		if(_memory->getSignature(id) && uContains(optimizedPoses, id))
 		{
 			Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
 			UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
 		}
+		UINFO("optimize time %f s", timer.ticks());
 	}
 }
 
+std::map<int, Transform> Rtabmap::optimizeGraph(
+		int fromId,
+		const std::vector<int> & ids,
+		bool lookInDatabase,
+		std::multimap<int, Link> * constraints) const
+{
+	UTimer timer;
+	std::map<int, Transform> optimizedPoses;
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> edgeConstraints;
+	UDEBUG("ids=%d", (int)ids.size());
+	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
+	UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
+
+	if(constraints)
+	{
+		*constraints = edgeConstraints;
+	}
+
+	UASSERT(_graphOptimizer!=0);
+	if(_graphOptimizer->iterations() == 0)
+	{
+		// Optimization desactivated! Return not optimized poses.
+		optimizedPoses = poses;
+	}
+	else
+	{
+		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
+	}
+
+	return optimizedPoses;
+}
+
 void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 {
 	ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());

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,6 +64,7 @@ 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
@@ -95,6 +97,7 @@ SensorData::SensorData(const cv::Mat & image,
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
 	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(0),
 	_userData(userData)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
@@ -109,6 +112,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,6 +139,7 @@ SensorData::SensorData(const cv::Mat & laserScan,
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
 	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(laserScanMaxPts),
 	_userData(userData)
 {
 	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);

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)
 {
 }
 
@@ -273,6 +276,7 @@ SensorData Signature::toSensorData()
 		}
 	}
 	return SensorData(_laserScanRaw,
+			_laserScanMaxPts,
 			_imageRaw,
 			_depthRaw,
 			_fx,

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

@@ -1933,7 +1933,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 +1956,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 +1986,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 +2019,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 +2046,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 +2076,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 +2108,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 +2135,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 +2165,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)
 		{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -178,6 +178,7 @@ 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

guilib/src/DatabaseViewer.cpp

@@ -671,44 +671,47 @@ 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;
+			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
-				ids = ids_;
+				Transform odomPose;
-			}
+				int weight = -1;
-			else
+				int mapId = -1;
-			{
+				std::string label;
-				for(int i=0; i<ids_.size(); ++i)
+				double stamp = 0;
+				std::vector<unsigned char> userData;
+				if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
 				{
-					Transform odomPose;
+					if(frameRate == 0 ||
-					int weight = -1;
+					   previousStamp == 0 ||
-					int mapId = -1;
+					   stamp == 0 ||
-					std::string label;
+					   stamp - previousStamp >= 1.0/frameRate)
-					double stamp = 0;
-					std::vector<unsigned char> userData;
-					if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
 					{
-						if(sessionExported == mapId)
+						if(sessionExported < 0 || sessionExported == mapId)
 						{
 							ids.push_back(ids_[i]);
 						}
-						else if(mapId > sessionExported)
+						previousStamp = stamp;
-						{
+					}
-							break;
+					if(sessionExported >= 0 && mapId > sessionExported)
-						}
+					{
+						break;
 					}
 				}
 			}
 			if(recorder.init(path, false))
 			{
 				rtabmap::DetailedProgressDialog progressDialog(this);
-				progressDialog.setMaximumSteps(ids.size() / (1+framesIgnored) + 1);
+				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);
 
@@ -1465,6 +1468,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();
 				}
 			}
@@ -1842,13 +1850,16 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
 void DatabaseViewer::updateConstraintView()
 {
 	Link link = this->findActiveLink(ui_->horizontalSlider_A->value(), ui_->horizontalSlider_B->value());
-	if(link.type() == Link::kNeighbor)
+	if(link.isValid())
 	{
-		this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
+		if(link.type() == Link::kNeighbor)
-	}
+		{
-	else
+			this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
-	{
+		}
-		this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+		else
+		{
+			this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
+		}
 	}
 }
 
@@ -2042,7 +2053,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())

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/MainWindow.cpp

@@ -981,7 +981,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));
 
@@ -2035,7 +2035,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
 
@@ -2735,7 +2735,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());
 

guilib/src/PreferencesDialog.cpp

@@ -299,6 +299,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()));
@@ -489,6 +491,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str());
 	_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str());
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
+	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str());
 
 	_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
 	_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
@@ -940,6 +943,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
@@ -1215,6 +1219,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");
@@ -1482,6 +1487,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");
@@ -2046,6 +2052,7 @@ void PreferencesDialog::selectSourceDatabase(bool user)
 		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
 		_ui->source_database_lineEdit_path->setText(path);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
+		_ui->source_checkBox_useDbStamps->setChecked(false);
 	}
 
 	if(_ui->groupBox_sourceDatabase->isChecked())
@@ -3060,6 +3067,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
 {

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

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-806</y>
+            <y>0</y>
             <width>760</width>
-            <height>1462</height>
+            <height>1029</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>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1603,6 +1603,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 +1634,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 +1651,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 +1661,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 +1671,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>
@@ -5244,14 +5261,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="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>
@@ -5271,6 +5288,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </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 scan 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>
@@ -5900,6 +5934,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 +5983,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 +6061,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 +6071,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 +6081,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 +6158,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 +6224,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.11</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>