0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 15)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/Camera.h

@@ -66,6 +66,7 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	void resetTimer();
 protected:
 	/**
 	 * Constructor

corelib/include/rtabmap/core/CameraRGB.h

@@ -68,7 +68,7 @@ public:
 	const CameraModel & cameraModel() const {return _model;}
 
 	void setPath(const std::string & dir) {_path=dir;}
-	void setStartIndex(int index) {_startAt = index;} // negative means last
+	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
 	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
 	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
 	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
@@ -123,6 +123,9 @@ public:
 		_groundTruthFormat = format;
 	}
 
+	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
+	double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
+
 	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
 	{
 		_isDepth = isDepth;
@@ -135,7 +138,8 @@ protected:
 			std::list<Transform> & outputPoses,
 			std::list<double> & stamps,
 			const std::string & filePath,
-			int format) const;
+			int format,
+			double maxTimeDiff) const;
 
 private:
 	std::string _path;
@@ -172,9 +176,9 @@ private:
 
 	std::string _odometryPath;
 	int _odometryFormat;
-
 	std::string _groundTruthPath;
 	int _groundTruthFormat;
+	double _maxPoseTimeDiff;
 
 	std::list<double> _stamps;
 	std::list<Transform> odometry_;

corelib/include/rtabmap/core/CameraRGBD.h

@@ -429,6 +429,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 

corelib/include/rtabmap/core/CameraStereo.h

@@ -188,6 +188,8 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 

corelib/include/rtabmap/core/Features2d.h

@@ -177,6 +177,8 @@ private:
 	int _subPixWinSize;
 	int _subPixIterations;
 	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
 	// Stereo stuff
 	Stereo * _stereo;
 };

corelib/include/rtabmap/core/OdometryInfo.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <map>
 #include "rtabmap/core/Transform.h"
 #include "rtabmap/core/RegistrationInfo.h"
+#include "rtabmap/core/CameraModel.h"
 #include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
@@ -68,6 +69,8 @@ public:
 		output.localBundleOutliers = localBundleOutliers;
 		output.localBundleConstraints = localBundleConstraints;
 		output.localBundleTime = localBundleTime;
+		output.localBundlePoses = localBundlePoses;
+		output.localBundleModels = localBundleModels;
 		output.keyFrameAdded = keyFrameAdded;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
@@ -90,6 +93,8 @@ public:
 	int localBundleOutliers;
 	int localBundleConstraints;
 	float localBundleTime;
+	std::map<int, Transform> localBundlePoses;
+	std::map<int, CameraModel> localBundleModels;
 	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;

corelib/include/rtabmap/core/Parameters.h

@@ -239,6 +239,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
     RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Kp, GridRows,                 int, 1,       uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
+    RTABMAP_PARAM(Kp, GridCols,                 int, 1,       uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
 
     //Database
     RTABMAP_PARAM(DbSqlite3, InMemory,     bool, false,      "Using database in the memory instead of a file on the hard disk.");
@@ -463,13 +465,14 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
 
     // Odometry ORB_SLAM2
-    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,            "", "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath,             "",    "Path to ORB vocabulary (*.txt).");
-    RTABMAP_PARAM(OdomORBSLAM2, Bf,          double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM2, Bf,              double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
-    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,     double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM2, ThDepth,         double, 40.0,  "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM2, Fps,             float,  0.0,   "Camera FPS.");
+    RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
 
     // Common registration parameters
-    RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+    RTABMAP_PARAM(Reg, RepeatOnce,               bool, false,   "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration. It can be useful if the registration approach used can use a guess to get better matches.");
-    RTABMAP_PARAM(Reg, VarianceNormalized,       bool, false,   "Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
     RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
 
@@ -501,6 +504,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,      "See cv::cornerSubPix().");
     RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,      "See cv::cornerSubPix(). 0 disables sub pixel refining.");
     RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, GridRows,                 int, 1,      uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, GridCols,                 int, 1,      uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
     RTABMAP_PARAM(Vis, CorType,                  int, 0,      "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
     RTABMAP_PARAM(Vis, CorNNType,                int, 1,    uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
     RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.6,  uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
@@ -635,7 +640,7 @@ public:
     static ParametersMap getDefaultParameters(const std::string & group);
     static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
 
-    static void readINI(const std::string & configFile, ParametersMap & parameters);
+    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);
 
     /**

corelib/include/rtabmap/core/Registration.h

@@ -45,6 +45,7 @@ public:
 		kTypeIcp = 1,
 		kTypeVisIcp = 2
 	};
+	static double COVARIANCE_EPSILON;
 
 public:
 	static Registration * create(const ParametersMap & parameters);
@@ -61,9 +62,8 @@ public:
 	int getMinVisualCorrespondences() const;
 	float getMinGeometryCorrespondencesRatio() const;
 
-	bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
+	bool repeatOnce() const {return repeatOnce_;}
 	bool force3DoF() const {return force3DoF_;}
-	bool covarianceNormalized() const {return covarianceNormalized_;}
 
 	// take ownership!
 	void setChildRegistration(Registration * child);
@@ -85,8 +85,6 @@ public:
 			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
-	void normalizeCovariance(cv::Mat & covariance, const Transform & transform) const;
-
 protected:
 	// take ownership of child
 	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
@@ -106,8 +104,7 @@ protected:
 	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
 
 private:
-	bool varianceFromInliersCount_;
+	bool repeatOnce_;
-	bool covarianceNormalized_;
 	bool force3DoF_;
 	Registration * child_;
 

corelib/include/rtabmap/core/SensorData.h

@@ -235,6 +235,8 @@ public:
 	long getMemoryUsed() const; // Return memory usage in Bytes
 	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed=cv::Mat(); _userDataCompressed=cv::Mat();}
 
+	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
+
 private:
 	int _id;
 	double _stamp;

corelib/include/rtabmap/core/Statistics.h

@@ -64,6 +64,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
+	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
 	RTABMAP_STATS(Loop, Optimization_error, );
 	RTABMAP_STATS(Loop, Optimization_iterations, );
 

corelib/include/rtabmap/core/util2d.h

@@ -107,7 +107,7 @@ float RTABMAP_EXP getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
 		bool smoothing,
-		float maxZError = 0.02f,
+		float depthErrorRatio = 0.02f, //ratio
 		bool estWithNeighborsIfNull = false);
 
 cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios);

corelib/include/rtabmap/core/util3d.h

@@ -72,7 +72,7 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		float cx, float cy,
 		float fx, float fy,
 		bool smoothing,
-		float maxZError = 0.02f);
+		float depthErrorRatio = 0.02f);
 
 RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,

corelib/src/Camera.cpp

@@ -59,6 +59,11 @@ Camera::~Camera()
 	UDEBUG("");
 }
 
+void Camera::resetTimer()
+{
+	_frameRateTimer->start();
+}
+
 SensorData Camera::takeImage(CameraInfo * info)
 {
 	bool warnFrameRateTooHigh = false;

corelib/src/CameraRGB.cpp

@@ -78,6 +78,7 @@ CameraImages::CameraImages() :
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
+		_maxPoseTimeDiff(0.02),
 		_captureDelay(0.0)
 	{}
 CameraImages::CameraImages(const std::string & path,
@@ -108,6 +109,7 @@ CameraImages::CameraImages(const std::string & path,
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
+	_maxPoseTimeDiff(0.02),
 	_captureDelay(0.0)
 {
 
@@ -312,12 +314,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 
 		if(success && _odometryPath.size())
 		{
-			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
+			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
 		}
 
 		if(success && _groundTruthPath.size())
 		{
-			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
+			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
 		}
 	}
 
@@ -326,7 +328,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }
 
-bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format) const
+bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
 {
 	outputPoses.clear();
 	std::map<int, Transform> poses;
@@ -380,16 +382,21 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 					double stampBeg = beginIter->first;
 					double stampEnd = endIter->first;
 					UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-					if(stampEnd - stampBeg > 10.0)
+					if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
 					{
 						warned = true;
-						UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
+						if(!warned)
-							*ster,
+						{
-							stampBeg,
+							UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
-							stampEnd);
+								*ster,
+								stampBeg,
+								stampEnd,
+								maxTimeDiff);
+						}
 					}
 					else
 					{
+						warned=false;
 						float t = (*ster - stampBeg) / (stampEnd-stampBeg);
 						Transform & ta = poses.at(beginIter->second);
 						Transform & tb = poses.at(endIter->second);
@@ -533,6 +540,26 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 					}
 				}
 			}
+
+			if(_stamps.size())
+			{
+				stamp = _stamps.front();
+				_stamps.pop_front();
+				if(_stamps.size())
+				{
+					_captureDelay = _stamps.front() - stamp;
+				}
+				if(odometry_.size())
+				{
+					odometryPose = odometry_.front();
+					odometry_.pop_front();
+				}
+				if(groundTruth_.size())
+				{
+					groundTruthPose = groundTruth_.front();
+					groundTruth_.pop_front();
+				}
+			}
 		}
 		else
 		{
@@ -541,9 +568,48 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			if(!fileName.empty())
 			{
 				imageFilePath = _path + fileName;
+				if(_stamps.size())
+				{
+					stamp = _stamps.front();
+					_stamps.pop_front();
+					if(_stamps.size())
+					{
+						_captureDelay = _stamps.front() - stamp;
+					}
+					if(odometry_.size())
+					{
+						odometryPose = odometry_.front();
+						odometry_.pop_front();
+					}
+					if(groundTruth_.size())
+					{
+						groundTruthPose = groundTruth_.front();
+						groundTruth_.pop_front();
+					}
+				}
+
 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
 					imageFilePath = _path + fileName;
+					if(_stamps.size())
+					{
+						stamp = _stamps.front();
+						_stamps.pop_front();
+						if(_stamps.size())
+						{
+							_captureDelay = _stamps.front() - stamp;
+						}
+						if(odometry_.size())
+						{
+							odometryPose = odometry_.front();
+							odometry_.pop_front();
+						}
+						if(groundTruth_.size())
+						{
+							groundTruthPose = groundTruth_.front();
+							groundTruth_.pop_front();
+						}
+					}
 				}
 			}
 			if(_scanDir)
@@ -560,26 +626,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}
 
-		if(_stamps.size())
-		{
-			stamp = _stamps.front();
-			_stamps.pop_front();
-			if(_stamps.size())
-			{
-				_captureDelay = _stamps.front() - stamp;
-			}
-			if(odometry_.size())
-			{
-				odometryPose = odometry_.front();
-				odometry_.pop_front();
-			}
-			if(groundTruth_.size())
-			{
-				groundTruthPose = groundTruth_.front();
-				groundTruth_.pop_front();
-			}
-		}
-
 		if(!imageFilePath.empty())
 		{
 			ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());

corelib/src/CameraThread.cpp

@@ -122,6 +122,7 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
+	_camera->resetTimer();
 }
 
 void CameraThread::mainLoop()

corelib/src/DBDriverSqlite3.cpp

@@ -1406,6 +1406,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 											localTransform));
 								}
 							}
+							else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
+							{
+								UDEBUG("Loading calibration of a stereo camera");
+								memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
+								stereoModel = StereoCameraModel(
+										dataFloat[0],  // fx
+										dataFloat[1],  // fy
+										dataFloat[2],  // cx
+										dataFloat[3],  // cy
+										dataFloat[4], // baseline
+										localTransform,
+										cv::Size(dataFloat[5],dataFloat[6]));
+							}
 							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 							{
 								UDEBUG("Loading calibration of a stereo camera");
@@ -1707,6 +1720,19 @@ bool DBDriverSqlite3::getCalibrationQuery(
 									localTransform));
 						}
 					}
+					else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
+					{
+						UDEBUG("Loading calibration of a stereo camera");
+						memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
+						stereoModel = StereoCameraModel(
+								dataFloat[0],  // fx
+								dataFloat[1],  // fy
+								dataFloat[2],  // cx
+								dataFloat[3],  // cy
+								dataFloat[4], // baseline
+								localTransform,
+								cv::Size(dataFloat[5],dataFloat[6]));
+					}
 					else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 					{
 						UDEBUG("Loading calibration of a stereo camera");
@@ -2667,7 +2693,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 					// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
-					// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+					// stereo [fx, fy, cx, cy, baseline, [width,height], local_transform] (5or7+12)*float
 					if(dataSize > 0 && data)
 					{
 						float * dataFloat = (float*)data;
@@ -2687,8 +2713,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 										(double)dataFloat[i+1],
 										(double)dataFloat[i+2],
 										(double)dataFloat[i+3],
-										localTransform));
+										localTransform,
-								models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+										0,
+										cv::Size(dataFloat[i+4], dataFloat[i+5])));
 								UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
 										dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
 										localTransform.prettyPrint().c_str());
@@ -2713,6 +2740,19 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 										localTransform));
 							}
 						}
+						else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
+						{
+							UDEBUG("Loading calibration of a stereo camera");
+							memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
+							stereoModel = StereoCameraModel(
+									dataFloat[0],  // fx
+									dataFloat[1],  // fy
+									dataFloat[2],  // cx
+									dataFloat[3],  // cy
+									dataFloat[4], // baseline
+									localTransform,
+									cv::Size(dataFloat[5], dataFloat[6]));
+						}
 						else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 						{
 							UDEBUG("Loading calibration of a stereo camera");
@@ -4805,13 +4845,15 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	else if(sensorData.stereoCameraModel().isValidForProjection())
 	{
 		const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
-		calibration.resize(5+localTransform.size());
+		calibration.resize(7+localTransform.size());
 		calibration[0] = sensorData.stereoCameraModel().left().fx();
 		calibration[1] = sensorData.stereoCameraModel().left().fy();
 		calibration[2] = sensorData.stereoCameraModel().left().cx();
 		calibration[3] = sensorData.stereoCameraModel().left().cy();
 		calibration[4] = sensorData.stereoCameraModel().baseline();
-		memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
+		calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
+		calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
+		memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
 	}
 
 	if(calibration.size())

corelib/src/Features2d.cpp

@@ -327,7 +327,9 @@ Feature2D::Feature2D(const ParametersMap & parameters) :
 		_roiRatios(std::vector<float>(4, 0.0f)),
 		_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
 		_subPixIterations(Parameters::defaultKpSubPixIterations()),
-		_subPixEps(Parameters::defaultKpSubPixEps())
+		_subPixEps(Parameters::defaultKpSubPixEps()),
+		gridRows_(Parameters::defaultKpGridRows()),
+		gridCols_(Parameters::defaultKpGridCols())
 {
 	_stereo = new Stereo(parameters);
 	this->parseParameters(parameters);
@@ -346,6 +348,14 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
 	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
 	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
+	Parameters::parse(parameters, Parameters::kKpGridRows(), gridRows_);
+	Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
+
+	UASSERT(gridRows_ >= 1 && gridCols_>=1);
+	if(maxFeatures_ > 0)
+	{
+		maxFeatures_ =	maxFeatures_ / (gridRows_ * gridCols_);
+	}
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -533,23 +543,36 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 
 	std::vector<cv::KeyPoint> keypoints;
 	UTimer timer;
+	cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios);
+	if(!(globalRoi.width && globalRoi.height))
+	{
+		globalRoi = cv::Rect(0,0,image.cols, image.rows);
+	}
 
 	// Get keypoints
-	cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
+	int rowSize = globalRoi.height / gridRows_;
-	keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
+	int colSize = globalRoi.width / gridCols_;
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
+	for (int i = 0; i<gridRows_; ++i)
-
-	limitKeypoints(keypoints, maxFeatures_);
-
-	if(roi.x || roi.y)
 	{
-		// Adjust keypoint position to raw image
+		for (int j = 0; j<gridCols_; ++j)
-		for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
 		{
-			iter->pt.x += roi.x;
+			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
-			iter->pt.y += roi.y;
+			std::vector<cv::KeyPoint> sub_keypoints;
+			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
+			limitKeypoints(sub_keypoints, maxFeatures_);
+			if(roi.x || roi.y)
+			{
+				// Adjust keypoint position to raw image
+				for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
+				{
+					iter->pt.x += roi.x;
+					iter->pt.y += roi.y;
+				}
+			}
+			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
 		}
 	}
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
 
 	if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
 	{

corelib/src/Memory.cpp

@@ -2251,7 +2251,7 @@ Transform Memory::computeTransform(
 
 	if(fromS && toS)
 	{
-		return computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
+		transform = computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
 	}
 	else
 	{
@@ -3515,7 +3515,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
 	{
-		UINFO("Use odometry features");
+		UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
+				(int)data.keypoints().size(),
+				(int)data.keypoints3D().size(),
+				data.descriptors().rows,
+				data.descriptors().cols,
+				data.descriptors().type());
 		keypoints = data.keypoints();
 		keypoints3D = data.keypoints3D();
 		descriptors = data.descriptors().clone();

corelib/src/Odometry.cpp

@@ -286,7 +286,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
-	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
+	// KITTI datasets start with stamp=0
+	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
 	if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
 	{
@@ -528,7 +529,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 
-		if(data.stamp() == 0)
+		if(data.stamp() == 0 && framesProcessed_ != 0)
 		{
 			UWARN("Null stamp detected");
 		}

corelib/src/OdometryDVO.cpp

@@ -216,8 +216,10 @@ Transform OdometryDVO::computeTransform(
 			lost_ = false;
 			cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
 			memcpy(information.data, result.Information.data(), 36*sizeof(double));
-			covariance = information.inv();
+			//copy only diagonal to avoid g2o/gtsam errors on graph optimization
-			covariance *= 100.0; // to be in the same scale than loop closure detection
+			covariance  = cv::Mat::eye(6,6,CV_64FC1);
+			covariance = information.inv().mul(covariance);
+			//covariance *= 100.0; // to be in the same scale than loop closure detection
 
 			Transform currentMotion = t;
 			t = motionFromKeyFrame_.inverse() * t;

corelib/src/OdometryF2M.cpp

@@ -111,7 +111,27 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
 
+	int corType = Parameters::defaultVisCorType();
+	Parameters::parse(parameters, Parameters::kVisCorType(), corType);
+	if(corType != 0)
+	{
+		UWARN("%s=%d is not supported by OdometryF2M, using Features matching approach instead (type=0).",
+				Parameters::kVisCorType().c_str(),
+				corType);
+		corType = 0;
+	}
+	uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
+
 	regPipeline_ = Registration::create(bundleParameters);
+	if(bundleAdjustment_>0 && !regPipeline_->isImageRequired())
+	{
+		UWARN("%s=%d cannot be used with registration not done with images (%s=%s), disabling bundle adjustment.",
+				Parameters::kOdomF2MBundleAdjustment().c_str(),
+				bundleAdjustment_,
+				Parameters::kRegStrategy().c_str(),
+				uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
+		bundleAdjustment_ = 0;
+	}
 }
 
 OdometryF2M::~OdometryF2M()
@@ -188,6 +208,7 @@ Transform OdometryF2M::computeTransform(
 			lastFrame_->sensorData().isValid())
 		{
 			Signature tmpMap = *map_;
+			UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
 			Transform transform = regPipeline_->computeTransformationMod(
 					tmpMap,
 					*lastFrame_,
@@ -255,10 +276,35 @@ Transform OdometryF2M::computeTransform(
 
 						UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
 								uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
-
+						cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
-						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
+						//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
+						//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
+						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
 
+						CameraModel model;
+						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
+						{
+							model = lastFrame_->sensorData().cameraModels()[0];
+						}
+						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
+						{
+							model = lastFrame_->sensorData().stereoCameraModel().left();
+							// Set Tx for stereo BA
+							model = CameraModel(model.fx(),
+									model.fy(),
+									model.cx(),
+									model.cy(),
+									model.localTransform(),
+									-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
+						}
+						else
+						{
+							UFATAL("no valid camera model!");
+						}
+						bundleModels.insert(std::make_pair(lastFrame_->id(), model));
+						Transform invLocalTransform = model.localTransform().inverse();
+
 						UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
 						std::map<int, std::map<int, cv::Point3f> > wordReferences;
 						for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
@@ -300,7 +346,9 @@ Transform OdometryF2M::computeTransform(
 							if(iter2D!=lastFrame_->getWords().end())
 							{
 								UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
-								references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, lastFrame_->getWords3().find(wordId)->second.x)));
+								//move back point in camera frame (to get depth along z)
+								cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
+								references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
 							}
 							wordReferences.insert(std::make_pair(wordId, references));
 
@@ -311,28 +359,6 @@ Transform OdometryF2M::computeTransform(
 							//}
 						}
 
-						CameraModel model;
-						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-						{
-							model = lastFrame_->sensorData().cameraModels()[0];
-						}
-						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-						{
-							model = lastFrame_->sensorData().stereoCameraModel().left();
-							// Set Tx for stereo BA
-							model = CameraModel(model.fx(),
-									model.fy(),
-									model.cx(),
-									model.cy(),
-									model.localTransform(),
-									-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
-						}
-						else
-						{
-							UFATAL("no valid camera model!");
-						}
-						bundleModels.insert(std::make_pair(lastFrame_->id(), model));
-
 						UDEBUG("sba...start");
 						// set root negative to fix all other poses
 						std::set<int> sbaOutliers;
@@ -343,6 +369,11 @@ Transform OdometryF2M::computeTransform(
 						totalBundleOutliers = (int)sbaOutliers.size();
 
 						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
+						if(info)
+						{
+							info->localBundlePoses = bundlePoses;
+							info->localBundleModels = bundleModels;
+						}
 
 						UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
 						if(bundlePoses.size() == bundlePoses_.size()+1)
@@ -406,7 +437,7 @@ Transform OdometryF2M::computeTransform(
 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
 						  visKeyFrameThr_ == 0 ||
-						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())) ||
+						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
 						  regInfo.inliers <= visKeyFrameThr_);
 				bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
 
@@ -455,6 +486,22 @@ Transform OdometryF2M::computeTransform(
 					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
 					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
 					std::set<int> seenStatusUpdated;
+					Transform invLocalTransform;
+					if(bundleAdjustment_>0)
+					{
+						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
+						{
+							invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
+						}
+						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
+						{
+							invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
+						}
+						else
+						{
+							UFATAL("no valid camera model!");
+						}
+					}
 					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
 						if(util3d::isFinite(iter->second))
@@ -480,16 +527,17 @@ Transform OdometryF2M::computeTransform(
 									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
 									iterBundlePosesRef->second += 1;
 
+									//move back point in camera frame (to get depth along z)
+									cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
 									if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 									{
 										std::map<int, cv::Point3f> framePt;
-
+										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
-										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
 										bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
+										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
 									}
 								}
 							}
@@ -510,15 +558,17 @@ Transform OdometryF2M::computeTransform(
 									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
 									iterBundlePosesRef->second += 1;
 
+									//move back point in camera frame (to get depth along z)
+									cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
 									if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
 									{
 										std::map<int, cv::Point3f> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
+										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
 										bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
+										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
 									}
 								}
 							}
@@ -568,18 +618,31 @@ Transform OdometryF2M::computeTransform(
 							}
 						}
 
+						Link * previousLink = 0;
 						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
 						{
-							if((iter->second <= 0 && // <= regPipeline_->getMinVisualCorrespondences() &&
+							if(iter->second <= 0)
-								bundlePoses_.begin()->first == iter->first)) // remove oldest pose first
 							{
-								UASSERT(bundlePoses_.erase(iter->first) == 1);
+								if(previousLink == 0 || bundleLinks_.find(iter->first) != bundleLinks_.end())
-								bundleLinks_.erase(iter->first);
+								{
-								bundleModels_.erase(iter->first);
+									if(previousLink)
-								bundlePoseReferences_.erase(iter++);
+									{
+										UASSERT(previousLink->to() == iter->first);
+										*previousLink = previousLink->merge(bundleLinks_.find(iter->first)->second, previousLink->type());
+									}
+									UASSERT(bundlePoses_.erase(iter->first) == 1);
+									bundleLinks_.erase(iter->first);
+									bundleModels_.erase(iter->first);
+									bundlePoseReferences_.erase(iter++);
+								}
 							}
 							else
 							{
+								previousLink=0;
+								if(bundleLinks_.find(iter->first) != bundleLinks_.end())
+								{
+									previousLink = &bundleLinks_.find(iter->first)->second;
+								}
 								++iter;
 							}
 						}
@@ -765,6 +828,7 @@ Transform OdometryF2M::computeTransform(
 
 					std::multimap<int, cv::KeyPoint> words;
 					std::multimap<int, cv::Point3f> transformedPoints;
+					std::multimap<int, int> mapPointWeights;
 					std::multimap<int, cv::Mat> descriptors;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
 					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
@@ -777,12 +841,27 @@ Transform OdometryF2M::computeTransform(
 						{
 							words.insert(*wordsIter);
 							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+							mapPointWeights.insert(std::make_pair(iter->first, 0));
 							descriptors.insert(*descIter);
 						}
 					}
 
 					if(bundleAdjustment_>0)
 					{
+						Transform invLocalTransform;
+						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
+						{
+							invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
+						}
+						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
+						{
+							invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
+						}
+						else
+						{
+							UFATAL("no valid camera model!");
+						}
+
 						// update bundleWordReferences_: used for bundle adjustment
 						for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 						{
@@ -795,9 +874,12 @@ Transform OdometryF2M::computeTransform(
 								float d = 0.0f;
 								if(lastFrame_->getWords3().count(iter->first) == 1)
 								{
-									d = lastFrame_->getWords3().find(iter->first)->second.x;
+									//move back point in camera frame (to get depth along z)
+									cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
+									d = pt3d.z;
 								}
 
+
 								framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
 								bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 							}

corelib/src/OdometryFovis.cpp

@@ -123,6 +123,7 @@ Transform OdometryFovis::computeTransform(
 		const Transform & guess,
 		OdometryInfo * info)
 {
+	UDEBUG("");
 	Transform t;
 
 #ifdef RTABMAP_FOVIS
@@ -205,6 +206,7 @@ Transform OdometryFovis::computeTransform(
 	Transform localTransform = Transform::getIdentity();
 	if(data.cameraModels().size() == 1) //depth
 	{
+		UDEBUG("");
 		fovis::CameraIntrinsicsParameters rgb_params;
 		memset(&rgb_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
 		rgb_params.width = data.cameraModels()[0].imageWidth();
@@ -263,6 +265,7 @@ Transform OdometryFovis::computeTransform(
 	}
 	else // stereo
 	{
+		UDEBUG("");
 		// initialize left camera parameters
 		fovis::CameraIntrinsicsParameters left_parameters;
 		left_parameters.width = data.stereoCameraModel().left().imageWidth();
@@ -331,6 +334,7 @@ Transform OdometryFovis::computeTransform(
 		fovis_ = new fovis::VisualOdometry(rect_, options);
 	}
 
+	UDEBUG("");
 	fovis_->processFrame(gray.data, depthSource);
 
 	// get the motion estimate for this frame to the previous frame.

corelib/src/OdometryORBSLAM2.cpp

@@ -591,6 +591,11 @@ public:
 		ofs << "Camera.RGB: 1" << std::endl;
 		ofs << std::endl;
 
+		float fps = rtabmap::Parameters::defaultOdomORBSLAM2Fps();
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Fps(), fps);
+		ofs << "Camera.fps: " << fps << std::endl;
+		ofs << std::endl;
+
 		//# Close/Far threshold. Baseline times.
 		double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth();
 		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth);
@@ -605,8 +610,8 @@ public:
 		//# ORB Parameters
 		//#--------------------------------------------------------------------------------------------
 		//# ORB Extractor: Number of features per image
-		int features = rtabmap::Parameters::defaultVisMaxFeatures();
+		int features = rtabmap::Parameters::defaultOdomORBSLAM2MaxFeatures();
-		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kVisMaxFeatures(), features);
+		rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MaxFeatures(), features);
 		ofs << "ORBextractor.nFeatures: " << features << std::endl;
 		ofs << std::endl;
 
@@ -869,14 +874,26 @@ Transform OdometryORBSLAM2::computeTransform(
 		}
 		else
 		{
-			//based on values set in viso2_ros
+			float baseline = data.stereoCameraModel().baseline();
+			if(baseline <= 0.0f)
+			{
+				baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
+				rtabmap::Parameters::parse(orbslam2_->parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
+			}
+			double linearVar = 0.0001;
+			if(baseline > 0.0f)
+			{
+				linearVar = baseline/8.0;
+				linearVar *= linearVar;
+			}
+
 			covariance = cv::Mat::eye(6,6, CV_64FC1);
-			covariance.at<double>(0,0) = 0.002;
+			covariance.at<double>(0,0) = linearVar;
-			covariance.at<double>(1,1) = 0.002;
+			covariance.at<double>(1,1) = linearVar;
-			covariance.at<double>(2,2) = 0.05;
+			covariance.at<double>(2,2) = linearVar;
-			covariance.at<double>(3,3) = 0.09;
+			covariance.at<double>(3,3) = 0.01;
-			covariance.at<double>(4,4) = 0.09;
+			covariance.at<double>(4,4) = 0.01;
-			covariance.at<double>(5,5) = 0.09;
+			covariance.at<double>(5,5) = 0.01;
 		}
 	}
 

corelib/src/OdometryThread.cpp

@@ -107,6 +107,7 @@ void OdometryThread::mainLoop()
 	if(getData(data))
 	{
 		OdometryInfo info;
+		UDEBUG("Processing data...");
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
 		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());

corelib/src/OptimizerG2O.cpp

@@ -646,6 +646,81 @@ std::map<int, Transform> OptimizerG2O::optimize(
 	return optimizedPoses;
 }
 
+#ifdef RTABMAP_ORB_SLAM2
+/**
+ * \brief 3D edge between two SBAcam
+ */
+ class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    EdgeSE3Expmap():  BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
+    bool read(std::istream& is)
+      {
+        return false;
+      }
+
+      bool write(std::ostream& os) const
+      {
+        return false;
+      }
+
+    void computeError()
+    {
+      const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+      const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+      g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+      _error[0]=delta.translation().x();
+      _error[1]=delta.translation().y();
+      _error[2]=delta.translation().z();
+      _error[3]=delta.rotation().x();
+      _error[4]=delta.rotation().y();
+      _error[5]=delta.rotation().z();
+    }
+
+    virtual void setMeasurement(const g2o::SE3Quat& meas){
+      _measurement=meas;
+      _inverseMeasurement=meas.inverse();
+    }
+
+    virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
+    virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
+    	g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
+    	g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
+		if (from_.count(from) > 0)
+		  to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
+		else
+		  from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
+    }
+
+    virtual bool setMeasurementData(const double* d){
+      Eigen::Map<const g2o::Vector7d> v(d);
+      _measurement.fromVector(v);
+      _inverseMeasurement = _measurement.inverse();
+      return true;
+    }
+
+    virtual bool getMeasurementData(double* d) const{
+      Eigen::Map<g2o::Vector7d> v(d);
+      v = _measurement.toVector();
+      return true;
+    }
+
+    virtual int measurementDimension() const {return 7;}
+
+    virtual bool setMeasurementFromState() {
+    	const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
+		const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
+		_measurement = (v1->estimate().inverse()*v2->estimate());
+		_inverseMeasurement = _measurement.inverse();
+		return true;
+    }
+
+  protected:
+    g2o::SE3Quat _inverseMeasurement;
+};
+#endif
+
 std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -761,7 +836,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			++iter;
 		}
 
-#ifndef RTABMAP_ORB_SLAM2
 		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
@@ -777,23 +851,36 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					UASSERT(!iter->second.transform().isNull());
 
 					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
-					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+					if(!isCovarianceIgnored())
+					{
+						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+					}
 
 					// between cameras, not base_link
 					Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-					UDEBUG("added edge %d->%d (in cam frame=%s)",
+					//UDEBUG("added edge %d->%d (in cam frame=%s)",
-							id1,
+					//		id1,
-							id2,
+					//		id2,
-							camLink.prettyPrint().c_str());
+					//		camLink.prettyPrint().c_str());
-					Eigen::Affine3d a = camLink.toEigen3d();
+#ifdef RTABMAP_ORB_SLAM2
+					EdgeSE3Expmap * e = new EdgeSE3Expmap();
+					g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
+					g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
 
+					Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
+					Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
+
+					camLink =  camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
+#else
 					g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
 					g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
 					g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
+#endif
 					UASSERT(v1 != 0);
 					UASSERT(v2 != 0);
 					e->setVertex(0, v1);
 					e->setVertex(1, v2);
+					Eigen::Affine3d a = camLink.toEigen3d();
 					e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
 					e->setInformation(information);
 
@@ -806,7 +893,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				}
 			}
 		}
-#endif
 
 		UDEBUG("fill 3D points to g2o...");
 		const int stepVertexId = poses.rbegin()->first+1;
@@ -815,7 +901,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		{
 			if(points3DMap.find(iter->first) != points3DMap.end())
 			{
-				const cv::Point3f & pt3d = points3DMap.at(iter->first);
+				cv::Point3f pt3d = points3DMap.at(iter->first);
 				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
 
 				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -823,7 +909,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);
 
-				UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
+				//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
 
 				// set observations
 				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
@@ -834,7 +920,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						const cv::Point3f & pt = jter->second;
 						double depth = pt.z;
 
-						UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
 
 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
@@ -842,18 +928,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
 						std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
 
-						cv::Point3f t = util3d::transformPoint(pt3d, Transform::fromEigen3d(vcam->estimate()).inverse());
-						UDEBUG("in cam %d frame=(%f,%f,%f)", camId, t.x, t.y, t.z);
-
-						cv::Point3f t2 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()).inverse());
-						UDEBUG("in cam2 %d frame=(%f,%f,%f)",camId, t2.x, t2.y, t2.z);
-
-						g2o::Vector3d t3 = vcam->estimate().map(g2o::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-						UDEBUG("in cam3 %d frame=(%f,%f,%f)",camId, t3[0], t3[1], t3[2]);
-
-						cv::Point3f t4 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()));
-						UDEBUG("in cam4 %d frame=(%f,%f,%f)",camId, t4.x, t4.y, t4.z);
-
 						UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
 						baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
 #else
@@ -918,7 +992,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						}
 						e->setVertex(0, vpt3d);
 						e->setVertex(1, vcam);
-						UDEBUG("");
+
 						if(robustKernelDelta_ > 0.0)
 						{
 							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
@@ -947,11 +1021,12 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
 		{
-			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?3:iterations());
+			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
 
 			// early stop condition
 			optimizer.computeActiveErrors();
 			double chi2 = optimizer.activeRobustChi2();
+
 			if(uIsNan(chi2))
 			{
 				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
@@ -988,7 +1063,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif
 
-						const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						cv::Point3f pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
 
 						if(outliers)
@@ -1007,12 +1082,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
 			}
 		}
-
 		UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
 
 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{
-			UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
+			UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
 			return optimizedPoses;
 		}
 
@@ -1034,6 +1108,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
+
 				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
 				if(t.isNull())
 				{

corelib/src/OptimizerGTSAM.cpp

@@ -160,16 +160,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 						if(!isCovarianceIgnored())
 						{
-							// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+							information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
-							information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
+							information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
-							information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
+							information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
-							information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
+							information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
-							information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
+							information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
-							information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
+							information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
-							information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
+							information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
-							information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
+							information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
-							information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
+							information(2,2) = iter->second.infMatrix().at<double>(5,5)/100000.0; // theta-theta
-							information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
 						}
 
 						gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -181,8 +180,10 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						if(!isCovarianceIgnored())
 						{
 							memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-							// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+							// Without the following, graph optimization on KITTI06 is very unstable
-							information = information / 1000.0;
+							information(3,3) /= 100000.0;
+							information(4,4) /= 100000.0;
+							information(5,5) /= 100000.0;
 						}
 
 						gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -221,16 +222,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 					if(!isCovarianceIgnored())
 					{
-						// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+						information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
-						information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
+						information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
-						information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
+						information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
-						information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
+						information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
-						information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
+						information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
-						information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
+						information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
-						information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
+						information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
-						information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
+						information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
-						information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
+						information(2,2) = iter->second.infMatrix().at<double>(5,5)/100000.0; // theta-theta
-						information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
 					}
 					gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
 
@@ -254,8 +254,10 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					if(!isCovarianceIgnored())
 					{
 						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-						// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+						// Without the following, graph optimization on KITTI06 is very unstable
-						information = information / 1000.0;
+						information(3,3) /= 100000.0;
+						information(4,4) /= 100000.0;
+						information(5,5) /= 100000.0;
 					}
 
 					gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);

corelib/src/Parameters.cpp

@@ -224,6 +224,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	if(removedParameters_.empty())
 	{
 		// removed parameters
+		// 0.15.1
+		removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount",  std::make_pair(false, "")));
+		removedParameters_.insert(std::make_pair("Reg/VarianceNormalized",        std::make_pair(false, "")));
 
 		// 0.13.3
 		removedParameters_.insert(std::make_pair("Icp/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneK())));
@@ -284,7 +287,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("Odom/MaxDepth",                 std::make_pair(true,  Parameters::kVisMaxDepth())));
 		removedParameters_.insert(std::make_pair("Odom/RoiRatios",                std::make_pair(true,  Parameters::kVisRoiRatios())));
 		removedParameters_.insert(std::make_pair("Odom/Force2D",                  std::make_pair(true,  Parameters::kRegForce3DoF())));
-		removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true,  Parameters::kRegVarianceFromInliersCount())));
+		removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("Odom/PnPReprojError",           std::make_pair(true,  Parameters::kVisPnPReprojError())));
 		removedParameters_.insert(std::make_pair("Odom/PnPFlags",                 std::make_pair(true,  Parameters::kVisPnPFlags())));
 
@@ -314,7 +317,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("LccBow/Iterations",               std::make_pair(false,  Parameters::kVisIterations())));
 		removedParameters_.insert(std::make_pair("LccBow/RefineIterations",         std::make_pair(false,  Parameters::kVisRefineIterations())));
 		removedParameters_.insert(std::make_pair("LccBow/Force2D",                  std::make_pair(false,  Parameters::kRegForce3DoF())));
-		removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false,  Parameters::kRegVarianceFromInliersCount())));
+		removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false,  "")));
 		removedParameters_.insert(std::make_pair("LccBow/PnPReprojError",           std::make_pair(false,  Parameters::kVisPnPReprojError())));
 		removedParameters_.insert(std::make_pair("LccBow/PnPFlags",                 std::make_pair(false,  Parameters::kVisPnPFlags())));
 		removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar",      std::make_pair(true,   Parameters::kVisEpipolarGeometryVar())));
@@ -732,7 +735,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 }
 
 
-void Parameters::readINI(const std::string & configFile, ParametersMap & parameters)
+void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
 {
 	CSimpleIniA ini;
 	ini.LoadFile(configFile.c_str());
@@ -805,7 +808,10 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 
 				if(Parameters::getDefaultParameters().find(key) != Parameters::getDefaultParameters().end())
 				{
-					uInsert(parameters, ParametersPair(key, iter->second));
+					if(!modifiedOnly || std::string(iter->second).compare(Parameters::getDefaultParameters().find(key)->second) != 0)
+					{
+						uInsert(parameters, ParametersPair(key, iter->second));
+					}
 				}
 			}
 		}

corelib/src/Registration.cpp

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+double Registration::COVARIANCE_EPSILON = 0.000000001;
+
 Registration * Registration::create(const ParametersMap & parameters)
 {
 	int regTypeInt = Parameters::defaultRegStrategy();
@@ -61,8 +63,7 @@ Registration * Registration::create(Registration::Type & type, const ParametersM
 }
 
 Registration::Registration(const ParametersMap & parameters, Registration * child) :
-	varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
+	repeatOnce_(Parameters::defaultRegRepeatOnce()),
-	covarianceNormalized_(Parameters::defaultRegVarianceNormalized()),
 	force3DoF_(Parameters::defaultRegForce3DoF()),
 	child_(child)
 {
@@ -78,9 +79,9 @@ Registration::~Registration()
 }
 void Registration::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
+	Parameters::parse(parameters, Parameters::kRegRepeatOnce(), repeatOnce_);
-	Parameters::parse(parameters, Parameters::kRegVarianceNormalized(), covarianceNormalized_);
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
+
 	if(child_)
 	{
 		child_->parseParameters(parameters);
@@ -194,26 +195,29 @@ Transform Registration::computeTransformationMod(
 	}
 
 	Transform t = computeTransformationImpl(from, to, guess, info);
+	if(repeatOnce_ && guess.isNull() && !t.isNull())
+	{
+		// redo with guess to get a more accurate transform
+		t = computeTransformationImpl(from, to, t, info);
+	}
 
 	if(info.covariance.empty())
 	{
 		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 
-	if(varianceFromInliersCount_)
+	if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
-	{
+		info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
-		if(info.icpInliersRatio)
+	if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
-		{
+		info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
-			info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0);
+	if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
-		}
+		info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
-		else
+	if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
-		{
+		info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
-			info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
+	if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
-		}
+		info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
-	}
+	if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
-
+		info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
-
-	normalizeCovariance(info.covariance, t);
 
 	if(child_)
 	{
@@ -239,36 +243,4 @@ Transform Registration::computeTransformationMod(
 	return t;
 }
 
-void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & transform) const
-{
-	UASSERT(covariance.cols == 6 && covariance.rows == 6);
-
-	if(covarianceNormalized_)
-	{
-		// normalize variance
-		float norm = transform.getNorm();
-		covariance.at<double>(0,0) *= norm;
-		covariance.at<double>(1,1) *= norm;
-		covariance.at<double>(2,2) *= norm;
-		float angle = transform.getAngle()/10.0;
-		covariance.at<double>(3,3) *= angle;
-		covariance.at<double>(4,4) *= angle;
-		covariance.at<double>(5,5) *= angle;
-	}
-
-	double epsilon = 0.000001;
-	if(covariance.at<double>(0,0)<=epsilon)
-		covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(1,1)<=epsilon)
-		covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(2,2)<=epsilon)
-		covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(3,3)<=epsilon)
-		covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(4,4)<=epsilon)
-		covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(5,5)<=epsilon)
-		covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
-}
-
 }

corelib/src/RegistrationVis.cpp

@@ -79,6 +79,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
 
 	this->parseParameters(parameters);
@@ -162,6 +164,14 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
 	}
+	if(uContains(parameters, Parameters::kVisGridRows()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
+	}
+	if(uContains(parameters, Parameters::kVisGridCols()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
+	}
 }
 
 RegistrationVis::~RegistrationVis()
@@ -425,7 +435,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom3D = kptsFrom3DKept;
 
 				std::vector<cv::Point3f> kptsTo3D;
-				if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
+				if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
 				{
 					kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
 				}
@@ -721,7 +731,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
 				}
-				isCalibrated = imageSize.height != 0 && imageSize.width != 0 && toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection();
+
+				isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
+						(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
 
 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -756,12 +768,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					std::vector<cv::Point2f> cornersProjected(projected.size());
 					std::vector<int> projectedIndexToDescIndex(projected.size());
 					int oi=0;
-					Transform guessInv = guess.inverse();
 					for(unsigned int i=0; i<projected.size(); ++i)
 					{
 						if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
 						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
-						   util3d::transformPoint(kptsFrom3D[i], guessInv).x > 0.0)
+						   util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
 						{
 							projectedIndexToDescIndex[oi] = i;
 							cornersProjected[oi++] = projected[i];
@@ -780,159 +791,273 @@ Transform RegistrationVis::computeTransformationImpl(
 					if(cornersProjected.size())
 					{
 
-						// Create kd-tree for projected keypoints
+						bool matchToProjected = false;
-						rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
+						if(matchToProjected)
-						rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
-						index.buildIndex();
-
-						std::vector< std::vector<size_t> > indices;
-						std::vector<std::vector<float> > dists;
-						float radius = (float)_guessWinSize; // pixels
-						std::vector<cv::Point2f> pointsTo;
-						cv::KeyPoint::convert(kptsTo, pointsTo);
-						rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
-						index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
-
-						UASSERT(indices.size() == pointsToMat.rows);
-						UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
-						UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
-						UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
-						UASSERT(pointsToMat.rows == kptsTo.size());
-						UDEBUG("radius search done for guess");
-
-						// Process results (Nearest Neighbor Distance Ratio)
-						int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
-						std::map<int,int> addedWordsFrom; //<id, index>
-						std::map<int, int> duplicates; //<fromId, toId>
-						int newWords = 0;
-						for(unsigned int i = 0; i < pointsToMat.rows; ++i)
 						{
-							if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
+							// match frame to projected
+							// Create kd-tree for projected keypoints
+							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
+							rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
+							index.buildIndex();
+
+							std::vector< std::vector<size_t> > indices;
+							std::vector<std::vector<float> > dists;
+							float radius = (float)_guessWinSize; // pixels
+							std::vector<cv::Point2f> pointsTo;
+							cv::KeyPoint::convert(kptsTo, pointsTo);
+							rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
+							index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
+
+							UASSERT(indices.size() == pointsToMat.rows);
+							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
+							UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
+							UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
+							UASSERT(pointsToMat.rows == kptsTo.size());
+							UDEBUG("radius search done for guess");
+
+							// Process results (Nearest Neighbor Distance Ratio)
+							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							std::map<int,int> addedWordsFrom; //<id, index>
+							std::map<int, int> duplicates; //<fromId, toId>
+							int newWords = 0;
+							for(unsigned int i = 0; i < pointsToMat.rows; ++i)
 							{
-								int octave = kptsTo[i].octave;
+								if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
-								int matchedIndex = -1;
-								if(indices[i].size() >= 2)
 								{
-									cv::Mat descriptors;
+									int octave = kptsTo[i].octave;
-									std::vector<int> descriptorsIndices(indices[i].size());
+									int matchedIndex = -1;
-									int oi=0;
+									if(indices[i].size() >= 2)
-									for(unsigned int j=0; j<indices[i].size(); ++j)
 									{
-										if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
+										cv::Mat descriptors;
+										std::vector<int> descriptorsIndices(indices[i].size());
+										int oi=0;
+										for(unsigned int j=0; j<indices[i].size(); ++j)
 										{
-											descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
+											if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
-											descriptorsIndices[oi++] = indices[i].at(j);
+											{
+												descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
+												descriptorsIndices[oi++] = indices[i].at(j);
+											}
+										}
+										descriptorsIndices.resize(oi);
+										if(oi >=2)
+										{
+											std::vector<std::vector<cv::DMatch> > matches;
+											cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+											matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
+											UASSERT(matches.size() == 1);
+											UASSERT(matches[0].size() == 2);
+											if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+											{
+												matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
+											}
+										}
+										else if(oi == 1)
+										{
+											matchedIndex = descriptorsIndices[0];
 										}
 									}
-									descriptorsIndices.resize(oi);
+									else if(indices[i].size() == 1 &&
-									if(oi >=2)
+											kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
 									{
-										std::vector<std::vector<cv::DMatch> > matches;
+										matchedIndex = indices[i].at(0);
-										cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+									}
-										matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
+
-										UASSERT(matches.size() == 1);
+									if(matchedIndex >= 0)
-										UASSERT(matches[0].size() == 2);
+									{
-										if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+										matchedIndex = projectedIndexToDescIndex[matchedIndex];
+										int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
+
+										if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
 										{
-											matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
+											id = addedWordsFrom.at(matchedIndex);
+											duplicates.insert(std::make_pair(matchedIndex, id));
 										}
-									}
+										else
-									else if(oi == 1)
+										{
-									{
+											addedWordsFrom.insert(std::make_pair(matchedIndex, id));
-										matchedIndex = descriptorsIndices[0];
-									}
-								}
-								else if(indices[i].size() == 1 &&
-									    kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
-								{
-									matchedIndex = indices[i].at(0);
-								}
 
-								if(matchedIndex >= 0)
+											if(kptsFrom.size())
-								{
+											{
-									matchedIndex = projectedIndexToDescIndex[matchedIndex];
+												wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
+											}
+											words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
+											wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
+										}
 
-									if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
+										wordsTo.insert(std::make_pair(id, kptsTo[i]));
-									{
+										wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
-										id = addedWordsFrom.at(matchedIndex);
+										if(kptsTo3D.size())
-										duplicates.insert(std::make_pair(matchedIndex, id));
+										{
+											words3To.insert(std::make_pair(id, kptsTo3D[i]));
+										}
 									}
 									else
 									{
-										addedWordsFrom.insert(std::make_pair(matchedIndex, id));
+										// gen fake ids
-
+										wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
-										if(kptsFrom.size())
+										wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
+										if(kptsTo3D.size())
 										{
-											wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
+											words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
 										}
-										words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
-										wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
-									}
 
-									wordsTo.insert(std::make_pair(id, kptsTo[i]));
+										++newToId;
-									wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
+										++newWords;
-									if(kptsTo3D.size())
-									{
-										words3To.insert(std::make_pair(id, kptsTo3D[i]));
 									}
 								}
-								else
+							}
+							UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
+								(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
+								(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
+
+							// create fake ids for not matched words from "from"
+							int addWordsFromNotMatched = 0;
+							for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
+							{
+								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
+								{
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+									wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
+									words3From.insert(std::make_pair(id, kptsFrom3D[i]));
+
+									++addWordsFromNotMatched;
+								}
+							}
+							UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
+						}
+						else
+						{
+							// match projected to frame
+							std::vector<cv::Point2f> pointsTo;
+							cv::KeyPoint::convert(kptsTo, pointsTo);
+							rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
+							rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
+							index.buildIndex();
+
+							std::vector< std::vector<size_t> > indices;
+							std::vector<std::vector<float> > dists;
+							float radius = (float)_guessWinSize; // pixels
+							rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
+							index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
+
+							UASSERT(indices.size() == cornersProjectedMat.rows);
+							UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
+							UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
+							UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
+							UASSERT(pointsToMat.rows == kptsTo.size());
+							UDEBUG("radius search done for guess");
+
+							// Process results (Nearest Neighbor Distance Ratio)
+							std::set<int> addedWordsTo;
+							std::set<int> addedWordsFrom;
+							std::set<int> indicesToIgnore;
+							for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
+							{
+								int matchedIndexFrom = projectedIndexToDescIndex[i];
+								if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
+								{
+									int matchedIndexTo = -1;
+									if(indices[i].size() >= 2)
+									{
+										cv::Mat descriptors;
+										std::vector<int> descriptorsIndices(indices[i].size());
+										int oi=0;
+										for(unsigned int j=0; j<indices[i].size(); ++j)
+										{
+											int octave = kptsTo[indices[i].at(j)].octave;
+											if(kptsFrom.at(matchedIndexFrom).octave==octave)
+											{
+												descriptors.push_back(descriptorsTo.row(indices[i].at(j)));
+												descriptorsIndices[oi++] = indices[i].at(j);
+
+												if(dists[i].at(j) < radius)
+												{
+													indicesToIgnore.insert(indices[i].at(j));
+												}
+											}
+										}
+										descriptorsIndices.resize(oi);
+										if(oi >=2)
+										{
+											std::vector<std::vector<cv::DMatch> > matches;
+											cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+											matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), descriptors, matches, 2);
+											UASSERT(matches.size() == 1);
+											UASSERT(matches[0].size() == 2);
+											if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+											{
+												matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
+											}
+										}
+										else if(oi == 1)
+										{
+											matchedIndexTo = descriptorsIndices[0];
+										}
+									}
+									else if(indices[i].size() == 1)
+									{
+										int octave = kptsTo[indices[i].at(0)].octave;
+										if(kptsFrom.at(matchedIndexFrom).octave == octave)
+										{
+											matchedIndexTo = indices[i].at(0);
+										}
+									}
+
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
+									addedWordsFrom.insert(matchedIndexFrom);
+
+									if(kptsFrom.size())
+									{
+										wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndexFrom]));
+									}
+									words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
+									wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
+
+									if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
+										matchedIndexTo >= 0 &&
+										addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
+									{
+										addedWordsTo.insert(matchedIndexTo);
+
+										wordsTo.insert(std::make_pair(id, kptsTo[matchedIndexTo]));
+										wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
+										if(kptsTo3D.size())
+										{
+											words3To.insert(std::make_pair(id, kptsTo3D[matchedIndexTo]));
+										}
+									}
+								}
+							}
+
+							// create fake ids for not matched words from "from"
+							for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
+							{
+								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
+								{
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+									wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
+									words3From.insert(std::make_pair(id, kptsFrom3D[i]));
+								}
+							}
+
+							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							for(unsigned int i = 0; i < kptsTo.size(); ++i)
+							{
+								if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
 								{
-									// gen fake ids
 									wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
 									wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
 									if(kptsTo3D.size())
 									{
 										words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
 									}
-
 									++newToId;
-									++newWords;
 								}
 							}
 						}
-
-						UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
-								(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
-								(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
-
-						// create fake ids for not matched words from "from"
-						int addWordsFromNotMatched = 0;
-						for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
-						{
-							if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
-							{
-								int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
-								wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
-								wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
-								words3From.insert(std::make_pair(id, kptsFrom3D[i]));
-
-								++addWordsFromNotMatched;
-							}
-						}
-						UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
-
-						/*std::vector<cv::KeyPoint> matches(wordsTo.size());
-						int oi=0;
-						for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
-						{
-							if(iter->first < (orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows) && wordsTo.count(iter->first) <= 1)
-							{
-								matches[oi++] = iter->second;
-							}
-						}
-						matches.resize(oi);
-						UDEBUG("guess=%s", guess.prettyPrint().c_str());
-						std::vector<cv::KeyPoint> projectedKpts;
-						cv::KeyPoint::convert(cornersProjected, projectedKpts);
-						cv::Mat image = toSignature.sensorData().imageRaw().clone();
-						drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
-						drawKeypoints(image, projectedKpts, image, cv::Scalar(0,255,255)); // BGR
-						drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
-						cv::imwrite("projected.bmp", image);
-						UWARN("saved projected.bmp");*/
-
 					}
 					else
 					{
@@ -1180,7 +1305,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 								uMultimapToMapUnique(signatureB->getWords3()),
-								varianceFromInliersCount()?0:&covariances[dir],
+								&covariances[dir],
 								&matchesV,
 								&inliersV);
 						inliers[dir] = inliersV;
@@ -1236,20 +1361,6 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
-
-			double epsilon = 0.000001;
-			if(covariances[dir].at<double>(0,0)<=epsilon)
-				covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
-			if(covariances[dir].at<double>(1,1)<=epsilon)
-				covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
-			if(covariances[dir].at<double>(2,2)<=epsilon)
-				covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
-			if(covariances[dir].at<double>(3,3)<=epsilon)
-				covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
-			if(covariances[dir].at<double>(4,4)<=epsilon)
-				covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
-			if(covariances[dir].at<double>(5,5)<=epsilon)
-				covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
 		}
 
 		if(!_forwardEstimateOnly)
@@ -1307,14 +1418,29 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(1, Transform::getIdentity()));
 			poses.insert(std::make_pair(2, transforms[0]));
 
+			for(int i=0;i<2;++i)
+			{
+				UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
+				if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
+				if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
+					covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
+			}
+
 			cv::Mat cov = covariances[0].clone();
-			normalizeCovariance(cov, transforms[0]);
 
 			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
 				cov = covariances[1].clone();
-				normalizeCovariance(cov, transforms[1]);
 				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
 			}
 
@@ -1325,6 +1451,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 			std::map<int, CameraModel> models;
 
+			Transform invLocalTransformFrom;
 			CameraModel cameraModelFrom;
 			if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
@@ -1336,12 +1463,15 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraModelFrom.cy(),
 						cameraModelFrom.localTransform(),
 						-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
+				invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
 			}
 			else if(fromSignature.sensorData().cameraModels().size() == 1)
 			{
 				cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
+				invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
 			}
 
+			Transform invLocalTransformTo = Transform::getIdentity();
 			CameraModel cameraModelTo;
 			if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
@@ -1353,10 +1483,16 @@ Transform RegistrationVis::computeTransformationImpl(
 						cameraModelTo.cy(),
 						cameraModelTo.localTransform(),
 						-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
+				invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
 			}
 			else if(toSignature.sensorData().cameraModels().size() == 1)
 			{
 				cameraModelTo = toSignature.sensorData().cameraModels()[0];
+				invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
+			}
+			if(invLocalTransformFrom.isNull())
+			{
+				invLocalTransformFrom = invLocalTransformTo;
 			}
 
 			models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
@@ -1372,14 +1508,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				std::map<int, cv::Point3f> ptMap;
 				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
 				{
+					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
 					const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
-					ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, pt3D.x)));
+					ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
 				}
 				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
 				{
+					float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
 					const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
 					UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
-					ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, toSignature.getWords3().find(wordId)->second.x)));
+					ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
 				}
 
 				wordReferences.insert(std::make_pair(wordId, ptMap));

corelib/src/Rtabmap.cpp

@@ -2170,6 +2170,7 @@ bool Rtabmap::process(
 	// Optimize map graph
 	//============================================================
 	float maxLinearError = 0.0f;
+	float maxLinearErrorRatio = 0.0f;
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	if(_rgbdSlamMode &&
@@ -2281,21 +2282,25 @@ bool Rtabmap::process(
 				}
 				if(maxLinearLink)
 				{
-					UINFO("Max optimization error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+					UINFO("Max optimization error = %f m (link %d->%d, var=%f, %f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
 				}
 
-				if(maxLinearError > _optimizationMaxLinearError)
+				float stddev = sqrt(maxLinearLink->transVariance());
+				maxLinearErrorRatio = maxLinearError/stddev;
+				if(maxLinearErrorRatio > _optimizationMaxLinearError)
 				{
 					UWARN("Rejecting all added loop closures (%d) in this "
 						  "iteration because a wrong loop closure has been "
 						  "detected after graph optimization, resulting in "
-						  "a maximum graph error of %f m (edge %d->%d, type=%d). The "
+						  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
-						  "maximum error parameter is %f m.",
+						  "maximum error ratio parameter is %f of std deviation.",
 						  (int)loopClosureLinksAdded.size(),
-						  maxLinearError,
+						  maxLinearErrorRatio,
 						  maxLinearLink->from(),
 						  maxLinearLink->to(),
 						  maxLinearLink->type(),
+						  maxLinearError,
+						  stddev,
 						  _optimizationMaxLinearError);
 					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
 					{
@@ -2384,6 +2389,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
+			statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
 			statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
 			statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
 

corelib/src/RtabmapThread.cpp

@@ -560,7 +560,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
-			if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
+			if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
 				((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
 			{
 				ignoreFrame = true;

corelib/src/SensorData.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Compression.h"
+#include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -793,5 +794,46 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_descriptors.total()*_descriptors.elemSize();
 }
 
+bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
+{
+	if(_cameraModels.size() >= 1)
+	{
+		for(unsigned int i=0; i<_cameraModels.size(); ++i)
+		{
+			if(_cameraModels[i].isValidForProjection() && !_cameraModels[i].localTransform().isNull())
+			{
+				cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
+				if(ptInCameraFrame.z > 0.0f)
+				{
+					int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
+					int u, v;
+					_cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
+					if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
+					   uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
+					{
+						return true;
+					}
+				}
+			}
+		}
+	}
+	else if(_stereoCameraModel.isValidForProjection())
+	{
+		cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
+		if(ptInCameraFrame.z > 0.0f)
+		{
+			int u, v;
+			_stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
+			return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
+				   uIsInBounds(v, 0, _stereoCameraModel.left().imageHeight());
+		}
+	}
+	else
+	{
+		UERROR("no valid camera model!");
+	}
+	return false;
+}
+
 } // namespace rtabmap
 

corelib/src/util2d.cpp

@@ -942,7 +942,7 @@ float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
 		bool smoothing,
-		float maxZError,
+		float depthErrorRatio,
 		bool estWithNeighborsIfNull)
 {
 	UASSERT(!depthImage.empty());
@@ -1024,10 +1024,15 @@ float getDepth(
 							tmp = d;
 							++count;
 						}
-						else if(fabs(d - tmp/float(count)) < maxZError)
+						else
 						{
-							tmp += d;
+							float depthError = depthErrorRatio * tmp;
-							++count;
+							if(fabs(d - tmp/float(count)) < depthError)
+
+							{
+								tmp += d;
+								++count;
+							}
 						}
 					}
 				}
@@ -1065,8 +1070,10 @@ float getDepth(
 							d = depthImage.at<float>(vv,uu);
 						}
 
+						float depthError = depthErrorRatio * depth;
+
 						// ignore if not valid or depth difference is too high
-						if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
+						if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < depthError)
 						{
 							if(uu == u || vv == v)
 							{

corelib/src/util3d.cpp

@@ -215,13 +215,13 @@ pcl::PointXYZ projectDepthTo3D(
 		float cx, float cy,
 		float fx, float fy,
 		bool smoothing,
-		float maxZError)
+		float depthErrorRatio)
 {
 	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
 
 	pcl::PointXYZ pt;
 
-	float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
+	float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio);
 	if(depth > 0.0f)
 	{
 		// Use correct principal point from calibration
@@ -2272,7 +2272,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
 		UASSERT(bytes % sizeof(float) == 0);
 		int32_t num = bytes/sizeof(float);
 		UASSERT(num % dim == 0);
-		float *data = (float*)malloc(num*sizeof(float));
+		float *data = new float[num];
 
 		// pointers
 		float *px = data+0;
@@ -2292,6 +2292,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
 			px+=4; py+=4; pz+=4; pr+=4;
 		}
 		fclose(stream);
+
+		delete[] data;
 	}
 
 	return cloud;

corelib/src/util3d_filtering.cpp

@@ -142,17 +142,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
-	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
-			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::VoxelGrid<pcl::PointXYZ> filter;
+	if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	if(indices->size())
 	{
-		filter.setIndices(indices);
+		pcl::VoxelGrid<pcl::PointXYZ> filter;
+		filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+		filter.setInputCloud(cloud);
+		if(indices->size())
+		{
+			filter.setIndices(indices);
+		}
+		filter.filter(*output);
 	}
-	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
@@ -161,17 +162,18 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
-	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
-			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
-	pcl::VoxelGrid<pcl::PointNormal> filter;
+	if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	if(indices->size())
 	{
-		filter.setIndices(indices);
+		pcl::VoxelGrid<pcl::PointNormal> filter;
+		filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+		filter.setInputCloud(cloud);
+		if(indices->size())
+		{
+			filter.setIndices(indices);
+		}
+		filter.filter(*output);
 	}
-	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
@@ -180,17 +182,18 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
-	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
-			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
+	if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	if(indices->size())
 	{
-		filter.setIndices(indices);
+		pcl::VoxelGrid<pcl::PointXYZRGB> filter;
+		filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+		filter.setInputCloud(cloud);
+		if(indices->size())
+		{
+			filter.setIndices(indices);
+		}
+		filter.filter(*output);
 	}
-	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
@@ -199,17 +202,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
-	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
-			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-	pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
+	if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	if(indices->size())
 	{
-		filter.setIndices(indices);
+		pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
+		filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+		filter.setInputCloud(cloud);
+		if(indices->size())
+		{
+			filter.setIndices(indices);
+		}
+		filter.filter(*output);
 	}
-	filter.filter(*output);
 	return output;
 }
 

corelib/src/util3d_motion_estimation.cpp

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_correspondences.h"
 #include "rtabmap/core/util3d.h"
 
+#include <pcl/common/common.h>
+
 #if CV_MAJOR_VERSION < 3
 #include "opencv/solvepnp.h"
 #endif
@@ -94,8 +96,8 @@ Transform estimateMotion3DTo2D(
 	imagePoints.resize(oi);
 	matches.resize(oi);
 
-	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d",
+	UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s",
-			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size());
+			(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), guess.prettyPrint().c_str());
 
 	if((int)matches.size() >= minInliers)
 	{
@@ -141,6 +143,7 @@ Transform estimateMotion3DTo2D(
 			if(covariance && words3B.size())
 			{
 				std::vector<float> errorSqrdDists(inliers.size());
+				std::vector<float> errorSqrdAngles(inliers.size());
 				oi = 0;
 				for(unsigned int i=0; i<inliers.size(); ++i)
 				{
@@ -150,19 +153,25 @@ Transform estimateMotion3DTo2D(
 						const cv::Point3f & objPt = objectPoints[inliers[i]];
 						cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
 						errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-						//ignore very very far features (stereo)
+
-						if(errorSqrdDists[oi] < iter->second.x/100.0f)
+						Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
-						{
+						Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
-							++oi;
+						errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2)*10.0f;
-						}
 					}
 				}
+
 				errorSqrdDists.resize(oi);
+				errorSqrdAngles.resize(oi);
 				if(errorSqrdDists.size())
 				{
 					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+					//divide by 4 instead of 2 to ignore very very far features (stereo)
-					*covariance *= 2.1981 * median_error_sqr;
+					double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+
+					(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
+					std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
+					median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
+					(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
 				}
 				else
 				{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -158,7 +158,6 @@ public:
 	bool isGroundTruthAligned() const;
 
 	bool isGraphsShown() const;
-	bool isFrustumsShown() const;
 	bool isLabelsShown() const;
 	double getVoxel() const;
 	double getNoiseRadius() const;
@@ -194,6 +193,7 @@ public:
 	int getScanPointSize(int index) const;     // 0=map, 1=odom
 
 	bool isFeaturesShown(int index) const;     // 0=map, 1=odom
+	bool isFrustumsShown(int index) const;     // 0=map, 1=odom
 	int getFeaturesPointSize(int index) const; // 0=map, 1=odom
 
 	bool isCloudFiltering() const;
@@ -267,7 +267,6 @@ public:
 	double getSimThr() const;
 	int getOdomStrategy() const;
 	int getOdomBufferSize() const;
-	bool getRegVarianceFromInliersCount() const;
 	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//
@@ -402,6 +401,7 @@ private:
 	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
 	QVector<QSpinBox*> _3dRenderingPtSizeScan;
 	QVector<QCheckBox*> _3dRenderingShowFeatures;
+	QVector<QCheckBox*> _3dRenderingShowFrustums;
 	QVector<QSpinBox*> _3dRenderingPtSizeFeatures;
 };
 

guilib/src/MainWindow.cpp

@@ -545,7 +545,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 		const std::map<std::string, float> & statistics = Statistics::defaultData();
 		for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
 		{
-			_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
+			// Don't add Gt panels yet if we don't know if we will receive Gt values.
+			if(!QString((*iter).first.c_str()).contains("Gt/"))
+			{
+				_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), false);
+			}
 		}
 	}
 	// Specific MainWindow
@@ -1187,6 +1191,43 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				}
 				featuresUpdated = true;
 			}
+
+			if(_preferencesDialog->isFrustumsShown(1))
+			{
+				QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
+				for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
+				{
+					std::list<std::string> splitted = uSplitNumChar(iter.key());
+					if(splitted.size() == 2)
+					{
+						int id = std::atoi(splitted.back().c_str());
+						if(splitted.front().compare("f_odom_") == 0 &&
+								odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
+						{
+							_cloudViewer->removeFrustum(iter.key());
+						}
+					}
+				}
+
+				for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
+				{
+					std::string frustumId = uFormat("f_odom_%d", iter->first);
+					if(_cloudViewer->getAddedFrustums().contains(frustumId))
+					{
+						_cloudViewer->updateFrustumPose(frustumId, _odometryCorrection*iter->second);
+					}
+					else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
+					{
+						const CameraModel & model = odom.info().localBundleModels.at(iter->first);
+						Transform t = model.localTransform();
+						if(!t.isNull())
+						{
+							QColor color = Qt::yellow;
+							_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color);
+						}
+					}
+				}
+			}
 		}
 		if(!dataIgnored)
 		{
@@ -1533,7 +1574,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 
 		// For intermediate empty nodes, keep latest image shown
-		if(!signature.sensorData().imageRaw().empty() || signature.getWords().size())
+		if(signature.getWeight() >= 0 &&
+		(!signature.sensorData().imageRaw().empty() || signature.getWords().size()))
 		{
 			_ui->imageView_source->clear();
 			_ui->imageView_loopClosure->clear();
@@ -1542,204 +1584,225 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			_ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
 
 			_ui->label_matchId->clear();
-		}
 
-		int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
-		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
-		int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
-		bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
-		_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
-		if(rehearsalMerged > 0)
+			int rehearsalMerged = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
-		{
+			bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
-			_ui->imageView_source->setBackgroundColor(Qt::blue);
+			int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
-		}
+			bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
-		else if(proximityTimeDetections > 0)
+			_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
-		{
-			_ui->imageView_source->setBackgroundColor(Qt::darkYellow);
-		}
-		else if(scanMatchingSuccess)
-		{
-			_ui->imageView_source->setBackgroundColor(Qt::darkCyan);
-		}
-		else if(rehearsedSimilarity)
-		{
-			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
-		}
-		else if(smallMovement)
-		{
-			_ui->imageView_source->setBackgroundColor(Qt::gray);
-		}
-		else if(fastMovement)
-		{
-			_ui->imageView_source->setBackgroundColor(Qt::magenta);
-		}
-		// Set color code as tooltip
-		if(_ui->label_refId->toolTip().isEmpty())
-		{
-			_ui->label_refId->setToolTip(
-				"Background Color Code:\n"
-				"  Blue = Weight Update Merged\n"
-				"  Dark Blue = Weight Update\n"
-				"  Dark Yellow = Proximity Detection in Time\n"
-				"  Dark Cyan = Neighbor Link Refined\n"
-				"  Gray = Small Movement\n"
-				"  Magenta = Fast Movement\n"
-				"Feature Color code:\n"
-				"  Green = New\n"
-				"  Yellow = New but Not Unique\n"
-				"  Red = In Vocabulary\n"
-		        "  Blue = In Vocabulary and in Previous Signature\n"
-				"  Pink = In Vocabulary and in Loop Closure Signature\n"
-				"  Gray = Not Quantized to Vocabulary");
-		}
-		// Set color code as tooltip
-		if(_ui->label_matchId->toolTip().isEmpty())
-		{
-			_ui->label_matchId->setToolTip(
-				"Background Color Code:\n"
-				"  Green = Accepted Loop Closure Detection\n"
-				"  Red = Rejected Loop Closure Detection\n"
-				"  Yellow = Proximity Detection in Space\n"
-				"Feature Color code:\n"
-				"  Red = In Vocabulary\n"
-				"  Pink = In Vocabulary and in Loop Closure Signature\n"
-				"  Gray = Not Quantized to Vocabulary");
-		}
 
-		UDEBUG("time= %d ms", time.restart());
+			if(rehearsalMerged > 0)
-
-		int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
-		float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
-		int matchId = 0;
-		Signature loopSignature;
-		int shownLoopId = 0;
-		if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
-		{
-			bool show = true;
-			if(stat.loopClosureId() > 0)
 			{
-				_ui->imageView_loopClosure->setBackgroundColor(Qt::green);
+				_ui->imageView_source->setBackgroundColor(Qt::blue);
-				_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
-				if(highestHypothesisIsSaved)
-				{
-					_ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
-				}
-				_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
-				matchId = stat.loopClosureId();
 			}
-			else if(stat.proximityDetectionId())
+			else if(proximityTimeDetections > 0)
 			{
-				_ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
+				_ui->imageView_source->setBackgroundColor(Qt::darkYellow);
-				_ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
-				matchId = stat.proximityDetectionId();
 			}
-			else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
+			else if(scanMatchingSuccess)
 			{
-				show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
+				_ui->imageView_source->setBackgroundColor(Qt::darkCyan);
-				if(show)
-				{
-					_ui->imageView_loopClosure->setBackgroundColor(Qt::red);
-					_ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
-					_ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
-				}
 			}
-			else
+			else if(rehearsedSimilarity)
 			{
-				show = _preferencesDialog->imageHighestHypShown();
+				_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
-				if(show)
+			}
-				{
+			else if(smallMovement)
-					_ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
+			{
-				}
+				_ui->imageView_source->setBackgroundColor(Qt::gray);
+			}
+			else if(fastMovement)
+			{
+				_ui->imageView_source->setBackgroundColor(Qt::magenta);
+			}
+			// Set color code as tooltip
+			if(_ui->label_refId->toolTip().isEmpty())
+			{
+				_ui->label_refId->setToolTip(
+					"Background Color Code:\n"
+					"  Blue = Weight Update Merged\n"
+					"  Dark Blue = Weight Update\n"
+					"  Dark Yellow = Proximity Detection in Time\n"
+					"  Dark Cyan = Neighbor Link Refined\n"
+					"  Gray = Small Movement\n"
+					"  Magenta = Fast Movement\n"
+					"Feature Color code:\n"
+					"  Green = New\n"
+					"  Yellow = New but Not Unique\n"
+					"  Red = In Vocabulary\n"
+					"  Blue = In Vocabulary and in Previous Signature\n"
+					"  Pink = In Vocabulary and in Loop Closure Signature\n"
+					"  Gray = Not Quantized to Vocabulary");
+			}
+			// Set color code as tooltip
+			if(_ui->label_matchId->toolTip().isEmpty())
+			{
+				_ui->label_matchId->setToolTip(
+					"Background Color Code:\n"
+					"  Green = Accepted Loop Closure Detection\n"
+					"  Red = Rejected Loop Closure Detection\n"
+					"  Yellow = Proximity Detection in Space\n"
+					"Feature Color code:\n"
+					"  Red = In Vocabulary\n"
+					"  Pink = In Vocabulary and in Loop Closure Signature\n"
+					"  Gray = Not Quantized to Vocabulary");
 			}
 
-			if(show)
-			{
-				shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
-				QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
-				if(iter != _cachedSignatures.end())
-				{
-					// uncompress after copy to avoid keeping uncompressed data in memory
-					loopSignature = iter.value();
-					loopSignature.sensorData().uncompressData();
-				}
-			}
-		}
-		_refIds.push_back(stat.refImageId());
-		_loopClosureIds.push_back(matchId);
-
-		//update image views
-		{
-			UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
-			UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
-			UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
-			UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
-			qimageThread.start();
-			qdepthThread.start();
-			qimageLoopThread.start();
-			qdepthLoopThread.start();
-			qimageThread.join();
-			qdepthThread.join();
-			qimageLoopThread.join();
-			qdepthLoopThread.join();
-			QImage img = qimageThread.getQImage();
-			QImage lcImg = qimageLoopThread.getQImage();
-			QImage depth = qdepthThread.getQImage();
-			QImage lcDepth = qdepthLoopThread.getQImage();
 			UDEBUG("time= %d ms", time.restart());
 
-			if(!img.isNull())
+			int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
+			float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
+			int matchId = 0;
+			Signature loopSignature;
+			int shownLoopId = 0;
+			if(highestHypothesisId > 0 || stat.proximityDetectionId()>0)
 			{
-				_ui->imageView_source->setImage(img);
+				bool show = true;
-			}
+				if(stat.loopClosureId() > 0)
-			if(!depth.isNull())
-			{
-				_ui->imageView_source->setImageDepth(depth);
-			}
-			if(img.isNull() && depth.isNull())
-			{
-				QRect sceneRect;
-				if(signature.sensorData().cameraModels().size())
 				{
-					for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
+					_ui->imageView_loopClosure->setBackgroundColor(Qt::green);
+					_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
+					if(highestHypothesisIsSaved)
 					{
-						sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
+						_ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
-						sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
+					}
+					_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
+					matchId = stat.loopClosureId();
+				}
+				else if(stat.proximityDetectionId())
+				{
+					_ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
+					_ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.proximityDetectionId()).arg(loopMapId));
+					matchId = stat.proximityDetectionId();
+				}
+				else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
+				{
+					show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();
+					if(show)
+					{
+						_ui->imageView_loopClosure->setBackgroundColor(Qt::red);
+						_ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
+						_ui->label_matchId->setText(QString("Loop hypothesis %1 rejected!").arg(highestHypothesisId));
 					}
 				}
-				else if(signature.sensorData().stereoCameraModel().isValidForProjection())
+				else
 				{
-					sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
+					show = _preferencesDialog->imageHighestHypShown();
+					if(show)
+					{
+						_ui->label_matchId->setText(QString("Highest hypothesis (%1)").arg(highestHypothesisId));
+					}
 				}
-				if(sceneRect.isValid())
+
+				if(show)
 				{
-					_ui->imageView_source->setSceneRect(sceneRect);
+					shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.proximityDetectionId()>0?stat.proximityDetectionId():highestHypothesisId;
+					QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
+					if(iter != _cachedSignatures.end())
+					{
+						// uncompress after copy to avoid keeping uncompressed data in memory
+						loopSignature = iter.value();
+						loopSignature.sensorData().uncompressData();
+					}
 				}
 			}
-			if(!lcImg.isNull())
+			_refIds.push_back(stat.refImageId());
+			_loopClosureIds.push_back(matchId);
+
+			//update image views
 			{
-				_ui->imageView_loopClosure->setImage(lcImg);
+				UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
+				UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
+				UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
+				UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
+				qimageThread.start();
+				qdepthThread.start();
+				qimageLoopThread.start();
+				qdepthLoopThread.start();
+				qimageThread.join();
+				qdepthThread.join();
+				qimageLoopThread.join();
+				qdepthLoopThread.join();
+				QImage img = qimageThread.getQImage();
+				QImage lcImg = qimageLoopThread.getQImage();
+				QImage depth = qdepthThread.getQImage();
+				QImage lcDepth = qdepthLoopThread.getQImage();
+				UDEBUG("time= %d ms", time.restart());
+
+				if(!img.isNull())
+				{
+					_ui->imageView_source->setImage(img);
+				}
+				if(!depth.isNull())
+				{
+					_ui->imageView_source->setImageDepth(depth);
+				}
+				if(img.isNull() && depth.isNull())
+				{
+					QRect sceneRect;
+					if(signature.sensorData().cameraModels().size())
+					{
+						for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
+						{
+							sceneRect.setWidth(sceneRect.width()+signature.sensorData().cameraModels()[i].imageWidth());
+							sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
+						}
+					}
+					else if(signature.sensorData().stereoCameraModel().isValidForProjection())
+					{
+						sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
+					}
+					if(sceneRect.isValid())
+					{
+						_ui->imageView_source->setSceneRect(sceneRect);
+					}
+				}
+				if(!lcImg.isNull())
+				{
+					_ui->imageView_loopClosure->setImage(lcImg);
+				}
+				if(!lcDepth.isNull())
+				{
+					_ui->imageView_loopClosure->setImageDepth(lcDepth);
+				}
+				if(_ui->imageView_loopClosure->sceneRect().isNull())
+				{
+					_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
+				}
 			}
-			if(!lcDepth.isNull())
+
+			UDEBUG("time= %d ms", time.restart());
+
+			// do it after scaling
+			this->drawKeypoints(signature.getWords(), loopSignature.getWords());
+
+			UDEBUG("time= %d ms", time.restart());
+
+			_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
+			_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
+
+			// loop closure view
+			if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0)  &&
+			   !stat.loopClosureTransform().isNull() &&
+			   !loopSignature.sensorData().imageRaw().empty())
 			{
-				_ui->imageView_loopClosure->setImageDepth(lcDepth);
+				// the last loop closure data
-			}
+				Transform loopClosureTransform = stat.loopClosureTransform();
-			if(_ui->imageView_loopClosure->sceneRect().isNull())
+				signature.setPose(loopClosureTransform);
-			{
+				_loopClosureViewer->setData(loopSignature, signature);
-				_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->sceneRect());
+				if(_ui->dockWidget_loopClosureViewer->isVisible())
+				{
+					UTimer loopTimer;
+					_loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
+					UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
+					_ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
+				}
+
+				UDEBUG("time= %d ms", time.restart());
 			}
 		}
 
-		UDEBUG("time= %d ms", time.restart());
-
-		// do it after scaling
-		this->drawKeypoints(signature.getWords(), loopSignature.getWords());
-
-		UDEBUG("time= %d ms", time.restart());
-
-		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
-		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
-
 		// PDF AND LIKELIHOOD
 		if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
 		{
@@ -1856,26 +1919,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
 			}
-
-			// loop closure view
-			if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0)  &&
-			   !stat.loopClosureTransform().isNull() &&
-			   !loopSignature.sensorData().imageRaw().empty())
-			{
-				// the last loop closure data
-				Transform loopClosureTransform = stat.loopClosureTransform();
-				signature.setPose(loopClosureTransform);
-				_loopClosureViewer->setData(loopSignature, signature);
-				if(_ui->dockWidget_loopClosureViewer->isVisible())
-				{
-					UTimer loopTimer;
-					_loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
-					UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
-					_ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(loopTimer.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
-				}
-
-				UDEBUG("time= %d ms", time.restart());
-			}
 		}
 
 		if( _ui->graphicsView_graphView->isVisible())
@@ -2284,11 +2327,22 @@ void MainWindow::updateMapCloud(
 	// update 3D graphes (show all poses)
 	_cloudViewer->removeAllGraphs();
 	_cloudViewer->removeCloud("graph_nodes");
-	if(!_preferencesDialog->isFrustumsShown())
+	if(!_preferencesDialog->isFrustumsShown(0))
 	{
-		_cloudViewer->removeAllFrustums(true);
+		QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
+		for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
+		{
+			std::list<std::string> splitted = uSplitNumChar(iter.key());
+			if(splitted.size() == 2)
+			{
+				if((splitted.front().compare("f_") == 0 || splitted.front().compare("f_gt_") == 0))
+				{
+					_cloudViewer->removeFrustum(iter.key());
+				}
+			}
+		}
 	}
-	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown()) && _currentPosesMap.size())
+	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
 	{
 		UTimer timerGraph;
 		// Find all graphs
@@ -2310,7 +2364,7 @@ void MainWindow::updateMapCloud(
 			}
 
 			// get local transforms for frustums on the graph
-			if(_preferencesDialog->isFrustumsShown())
+			if(_preferencesDialog->isFrustumsShown(0))
 			{
 				std::string frustumId = uFormat("f_%d", iter->first);
 				if(_cloudViewer->getAddedFrustums().contains(frustumId))
@@ -2370,7 +2424,7 @@ void MainWindow::updateMapCloud(
 			_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
 
-		if(_preferencesDialog->isFrustumsShown())
+		if(_preferencesDialog->isFrustumsShown(0))
 		{
 			QMap<std::string, Transform> addedFrustums = _cloudViewer->getAddedFrustums();
 			UDEBUG("remove not used frustums");
@@ -3281,6 +3335,11 @@ Transform MainWindow::alignPosesToGroundTruth(
 		if(_preferencesDialog->isGroundTruthAligned())
 		{
 			t = gtToMap;
+
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				iter->second = gtToMap * iter->second;
+			}
 		}
 
 		// ground truth live statistics
@@ -4656,7 +4715,7 @@ void MainWindow::pauseDetection()
 			emit stateChanged(kPaused);
 			if(_preferencesDialog->getGeneralInputRate())
 			{
-				QTimer::singleShot(1000.0/_preferencesDialog->getGeneralInputRate() + 10, this, SLOT(pauseDetection()));
+				QTimer::singleShot(500.0/_preferencesDialog->getGeneralInputRate(), this, SLOT(pauseDetection()));
 			}
 			else
 			{

guilib/src/PreferencesDialog.cpp

@@ -295,6 +295,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
 	_ui->checkBox_showFrustums->setEnabled(false);
 	_ui->checkBox_showFrustums->setChecked(false);
+	_ui->checkBox_showOdomFrustums->setEnabled(false);
+	_ui->checkBox_showOdomFrustums->setChecked(false);
 #endif
 
 	// in case we change the ui, we should not forget to change stuff related to this parameter
@@ -393,6 +395,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingShowFeatures[0] = _ui->checkBox_showFeatures;
 	_3dRenderingShowFeatures[1] = _ui->checkBox_showOdomFeatures;
 
+	_3dRenderingShowFrustums.resize(2);
+	_3dRenderingShowFrustums[0] = _ui->checkBox_showFrustums;
+	_3dRenderingShowFrustums[1] = _ui->checkBox_showOdomFrustums;
+
 	_3dRenderingPtSizeFeatures.resize(2);
 	_3dRenderingPtSizeFeatures[0] = _ui->spinBox_ptsize_features;
 	_3dRenderingPtSizeFeatures[1] = _ui->spinBox_ptsize_odom_features;
@@ -406,6 +412,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingRoiRatios[i], SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowFeatures[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingShowFrustums[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 		connect(_3dRenderingDownsamplingScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingVoxelSizeScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -428,7 +435,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_normalRadiusSearch_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->radioButton_noFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -470,6 +476,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	//Source panel
 	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), _ui->doubleSpinBox_OdomORBSLAM2Fps, SLOT(setValue(double)));
 	connect(_ui->source_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_source_path_calibration, SIGNAL(clicked()), this, SLOT(selectCalibrationPath()));
 	connect(_ui->lineEdit_calibrationFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -488,7 +495,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_images_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceImagesPath()));
 	connect(_ui->source_images_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_bayerMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	//video group
 	connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath()));
@@ -544,6 +550,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_cameraImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_cameraImages_syncTimeStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraRGBDImages_scale, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraRGBDImages_startIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -553,6 +560,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_maxPoseTimeDiff, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->groupBox_depthFromScan, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->groupBox_depthFromScan_fillHoles, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->radioButton_depthFromScan_vertical, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
@@ -564,6 +572,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_cameraStereoImages_path_left, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraStereoImages_path_right, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraStereoImages_startIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->toolButton_cameraStereoVideo_path, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath()));
 	connect(_ui->toolButton_cameraStereoVideo_path_2, SIGNAL(clicked()), this, SLOT(selectSourceStereoVideoPath2()));
@@ -699,6 +708,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
 	_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
+	_ui->spinBox_KPGridRows->setObjectName(Parameters::kKpGridRows().c_str());
+	_ui->spinBox_KPGridCols->setObjectName(Parameters::kKpGridCols().c_str());
 	_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
 	_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
 	_ui->checkBox_kp_parallelized->setObjectName(Parameters::kKpParallelized().c_str());
@@ -833,8 +844,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());
 
 	// Registration
-	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
+	_ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str());
-	_ui->reg_varianceNormalized->setObjectName(Parameters::kRegVarianceNormalized().c_str());
 	_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
 	_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().c_str());
 
@@ -859,6 +869,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
 	_ui->reextract_type->setObjectName(Parameters::kVisFeatureType().c_str());
 	_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
+	_ui->reextract_gridrows->setObjectName(Parameters::kVisGridRows().c_str());
+	_ui->reextract_gridcols->setObjectName(Parameters::kVisGridCols().c_str());
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
 	_ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str());
 	_ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str());
@@ -929,6 +941,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
 	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
 	_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
+	_ui->stackedWidget_odometryType->setCurrentIndex(Parameters::defaultOdomStrategy());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
@@ -1023,6 +1036,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_OdomORBSLAM2VocPath, SIGNAL(clicked()), this, SLOT(changeOdometryORBSLAM2Vocabulary()));
 	_ui->doubleSpinBox_OdomORBSLAM2Bf->setObjectName(Parameters::kOdomORBSLAM2Bf().c_str());
 	_ui->doubleSpinBox_OdomORBSLAM2ThDepth->setObjectName(Parameters::kOdomORBSLAM2ThDepth().c_str());
+	_ui->doubleSpinBox_OdomORBSLAM2Fps->setObjectName(Parameters::kOdomORBSLAM2Fps().c_str());
+	_ui->spinBox_OdomORBSLAM2MaxFeatures->setObjectName(Parameters::kOdomORBSLAM2MaxFeatures().c_str());
 
 	//Stereo
 	_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
@@ -1384,6 +1399,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingRoiRatios[i]->setText("0.0 0.0 0.0 0.0");
 			_3dRenderingShowScans[i]->setChecked(true);
 			_3dRenderingShowFeatures[i]->setChecked(i==0?false:true);
+			_3dRenderingShowFrustums[i]->setChecked(false);
 
 			_3dRenderingDownsamplingScan[i]->setValue(1);
 			_3dRenderingVoxelSizeScan[i]->setValue(0.0);
@@ -1408,7 +1424,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(0.0);
 
 		_ui->checkBox_showGraphs->setChecked(true);
-		_ui->checkBox_showFrustums->setChecked(false);
 		_ui->checkBox_showLabels->setChecked(false);
 
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
@@ -1470,7 +1485,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice);
 		_ui->source_images_spinBox_startPos->setValue(0);
-		_ui->source_images_refreshDir->setChecked(false);
 		_ui->checkBox_rgb_rectify->setChecked(false);
 		_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(0);
 
@@ -1537,6 +1551,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraRGBDImages_path_rgb->setText("");
 		_ui->lineEdit_cameraRGBDImages_path_depth->setText("");
 		_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(1.0);
+		_ui->spinBox_cameraRGBDImages_startIndex->setValue(0);
 		_ui->lineEdit_source_distortionModel->setText("");
 		_ui->groupBox_bilateral->setChecked(false);
 		_ui->doubleSpinBox_bilateral_sigmaS->setValue(10.0);
@@ -1546,6 +1561,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraStereoImages_path_left->setText("");
 		_ui->lineEdit_cameraStereoImages_path_right->setText("");
 		_ui->checkBox_stereo_rectify->setChecked(false);
+		_ui->spinBox_cameraStereoImages_startIndex->setValue(0);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
 		_ui->lineEdit_cameraStereoVideo_path_2->setText("");
 		_ui->comboBox_stereoZed_resolution->setCurrentIndex(2);
@@ -1568,6 +1584,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
 		_ui->lineEdit_cameraImages_gt->setText("");
 		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
+		_ui->doubleSpinBox_maxPoseTimeDiff->setValue(0.02);
 
 		_ui->groupBox_scanFromDepth->setChecked(false);
 		_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
@@ -1786,6 +1803,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingRoiRatios[i]->setText(settings.value(QString("roiRatios%1").arg(i), _3dRenderingRoiRatios[i]->text()).toString());
 		_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
 		_3dRenderingShowFeatures[i]->setChecked(settings.value(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked()).toBool());
+		_3dRenderingShowFrustums[i]->setChecked(settings.value(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked()).toBool());
 
 		_3dRenderingDownsamplingScan[i]->setValue(settings.value(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value()).toInt());
 		_3dRenderingVoxelSizeScan[i]->setValue(settings.value(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value()).toDouble());
@@ -1808,7 +1826,6 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(settings.value("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value()).toDouble());
 
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
-	_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
 
 	_ui->radioButton_noFiltering->setChecked(settings.value("noFiltering", _ui->radioButton_noFiltering->isChecked()).toBool());
@@ -1920,12 +1937,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_cameraRGBDImages_path_rgb->setText(settings.value("path_rgb", _ui->lineEdit_cameraRGBDImages_path_rgb->text()).toString());
 	_ui->lineEdit_cameraRGBDImages_path_depth->setText(settings.value("path_depth", _ui->lineEdit_cameraRGBDImages_path_depth->text()).toString());
 	_ui->doubleSpinBox_cameraRGBDImages_scale->setValue(settings.value("scale", _ui->doubleSpinBox_cameraRGBDImages_scale->value()).toDouble());
+	_ui->spinBox_cameraRGBDImages_startIndex->setValue(settings.value("start_index", _ui->spinBox_cameraRGBDImages_startIndex->value()).toInt());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
 	_ui->lineEdit_cameraStereoImages_path_left->setText(settings.value("path_left", _ui->lineEdit_cameraStereoImages_path_left->text()).toString());
 	_ui->lineEdit_cameraStereoImages_path_right->setText(settings.value("path_right", _ui->lineEdit_cameraStereoImages_path_right->text()).toString());
 	_ui->checkBox_stereo_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereo_rectify->isChecked()).toBool());
+	_ui->spinBox_cameraStereoImages_startIndex->setValue(settings.value("start_index",_ui->spinBox_cameraStereoImages_startIndex->value()).toInt());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -1947,7 +1966,6 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
-	_ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool());
 	_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(settings.value("bayerMode",_ui->comboBox_cameraImages_bayerMode->currentIndex()).toInt());
 
 	_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
@@ -1962,6 +1980,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt());
 	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
 	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
+	_ui->doubleSpinBox_maxPoseTimeDiff->setValue(settings.value("max_pose_time_diff", _ui->doubleSpinBox_maxPoseTimeDiff->value()).toDouble());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -2186,6 +2205,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("roiRatios%1").arg(i), _3dRenderingRoiRatios[i]->text());
 		settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
 		settings.setValue(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked());
+		settings.setValue(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked());
 
 		settings.setValue(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value());
 		settings.setValue(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value());
@@ -2208,7 +2228,6 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value());
 
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
-	settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
 
 	settings.setValue("noFiltering",             _ui->radioButton_noFiltering->isChecked());
@@ -2322,12 +2341,14 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("path_rgb",            _ui->lineEdit_cameraRGBDImages_path_rgb->text());
 	settings.setValue("path_depth",          _ui->lineEdit_cameraRGBDImages_path_depth->text());
 	settings.setValue("scale",               _ui->doubleSpinBox_cameraRGBDImages_scale->value());
+	settings.setValue("start_index",         _ui->spinBox_cameraRGBDImages_startIndex->value());
 	settings.endGroup(); // RGBDImages
 
 	settings.beginGroup("StereoImages");
 	settings.setValue("path_left",      _ui->lineEdit_cameraStereoImages_path_left->text());
 	settings.setValue("path_right",     _ui->lineEdit_cameraStereoImages_path_right->text());
 	settings.setValue("rectify", 	    _ui->checkBox_stereo_rectify->isChecked());
+	settings.setValue("start_index",    _ui->spinBox_cameraStereoImages_startIndex->value());
 	settings.endGroup(); // StereoImages
 
 	settings.beginGroup("StereoVideo");
@@ -2350,7 +2371,6 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.beginGroup("Images");
 	settings.setValue("path", 			_ui->source_images_lineEdit_path->text());
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
-	settings.setValue("refreshDir", 	_ui->source_images_refreshDir->isChecked());
 	settings.setValue("bayerMode", 	    _ui->comboBox_cameraImages_bayerMode->currentIndex());
 	settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
 	settings.setValue("sync_stamps",    _ui->checkBox_cameraImages_syncTimeStamps->isChecked());
@@ -2364,6 +2384,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("odom_format",         _ui->comboBox_cameraImages_odomFormat->currentIndex());
 	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
 	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
+	settings.setValue("max_pose_time_diff",  _ui->doubleSpinBox_maxPoseTimeDiff->value());
 	settings.endGroup(); // images
 
 	settings.beginGroup("Video");
@@ -4046,6 +4067,8 @@ void PreferencesDialog::useOdomFeatures()
 			_ui->surf_doubleSpinBox_maxDepth->setValue(_ui->loopClosure_bowMaxDepth->value());
 			_ui->surf_doubleSpinBox_minDepth->setValue(_ui->loopClosure_bowMinDepth->value());
 			_ui->surf_spinBox_wordsPerImageTarget->setValue(_ui->reextract_maxFeatures->value());
+			_ui->spinBox_KPGridRows->setValue(_ui->reextract_gridrows->value());
+			_ui->spinBox_KPGridCols->setValue(_ui->reextract_gridcols->value());
 			_ui->lineEdit_kp_roi->setText(_ui->loopClosure_roi->text());
 			_ui->subpix_winSize_kp->setValue(_ui->subpix_winSize->value());
 			_ui->subpix_iterations_kp->setValue(_ui->subpix_iterations->value());
@@ -4368,10 +4391,6 @@ bool PreferencesDialog::isGraphsShown() const
 {
 	return _ui->checkBox_showGraphs->isChecked();
 }
-bool PreferencesDialog::isFrustumsShown() const
-{
-	return _ui->checkBox_showFrustums->isEnabled() && _ui->checkBox_showFrustums->isChecked();
-}
 bool PreferencesDialog::isLabelsShown() const
 {
 	return _ui->checkBox_showLabels->isChecked();
@@ -4471,6 +4490,11 @@ bool PreferencesDialog::isFeaturesShown(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingShowFeatures[index]->isChecked();
 }
+bool PreferencesDialog::isFrustumsShown(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingShowFrustums[index]->isEnabled() && _3dRenderingShowFrustums[index]->isChecked();
+}
 int PreferencesDialog::getFeaturesPointSize(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);
@@ -4812,9 +4836,11 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+		((CameraRGBDImages*)camera)->setStartIndex(_ui->spinBox_cameraRGBDImages_startIndex->value());
 		((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraRGBDImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
 		((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraRGBDImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
@@ -4858,9 +4884,11 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			_ui->checkBox_stereo_rectify->isChecked() && !useRawImages,
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
+		((CameraStereoImages*)camera)->setStartIndex(_ui->spinBox_cameraStereoImages_startIndex->value());
 		((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
 		((CameraStereoImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
 		((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraStereoImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraStereoImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
@@ -4957,7 +4985,6 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			this->getSourceLocalTransform());
 
 		((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
-		((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
 		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgb_rectify->isChecked() && !useRawImages);
 
 		((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
@@ -4967,6 +4994,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		((CameraImages*)camera)->setGroundTruthPath(
 				_ui->lineEdit_cameraImages_gt->text().toStdString(),
 				_ui->comboBox_cameraImages_gtFormat->currentIndex());
+		((CameraImages*)camera)->setMaxPoseTimeDiff(_ui->doubleSpinBox_maxPoseTimeDiff->value());
 		((CameraImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
@@ -5076,10 +5104,6 @@ int PreferencesDialog::getOdomBufferSize() const
 {
 	return _ui->odom_dataBufferSize->value();
 }
-bool PreferencesDialog::getRegVarianceFromInliersCount() const
-{
-	return _ui->loopClosure_bowVarianceFromInliersCount->isChecked();
-}
 
 QString PreferencesDialog::getCameraInfoDir() const
 {

guilib/src/ui/preferencesDialog.ui

@@ -64,15 +64,24 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>673</width>
+            <width>678</width>
-            <height>2749</height>
+            <height>2739</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -86,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>23</number>
+                <number>20</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1641,6 +1650,16 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="4" column="1">
+                        <widget class="QCheckBox" name="checkBox_showOdomFrustums">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -2741,7 +2760,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>2</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2856,7 +2875,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>8</number>
+                              <number>7</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3612,7 +3631,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="1">
+                                  <item row="4" column="1">
                                    <spacer name="verticalSpacer_40">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -3625,6 +3644,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </spacer>
                                   </item>
+                                  <item row="3" column="2">
+                                   <widget class="QLabel" name="label_441">
+                                    <property name="text">
+                                     <string>Start position (index).</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="3" column="1">
+                                   <widget class="QSpinBox" name="spinBox_cameraRGBDImages_startIndex">
+                                    <property name="maximum">
+                                     <number>9999999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -3889,7 +3928,7 @@ 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">
                                    <spacer name="verticalSpacer_38">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -3949,6 +3988,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="2" column="2">
+                                   <widget class="QLabel" name="label_442">
+                                    <property name="text">
+                                     <string>Start position (index).</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="2" column="1">
+                                   <widget class="QSpinBox" name="spinBox_cameraStereoImages_startIndex">
+                                    <property name="maximum">
+                                     <number>9999999</number>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -4398,30 +4457,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="2" column="1">
-                                   <widget class="QLabel" name="label_34">
-                                    <property name="text">
-                                     <string>Refresh the directory files list after each image loaded.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="2" column="0">
-                                   <widget class="QCheckBox" name="source_images_refreshDir">
-                                    <property name="text">
-                                     <string/>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="1" column="1">
                                    <widget class="QLabel" name="label_21">
                                     <property name="text">
-                                     <string>Start position (default 1, 0=start from the last).</string>
+                                     <string>Start position (index).</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
@@ -4594,7 +4633,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item row="5" column="1">
                             <widget class="QLabel" name="label_58">
                              <property name="text">
-                              <string>Start position (index)</string>
+                              <string>Start position (index).</string>
                              </property>
                              <property name="textInteractionFlags">
                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
@@ -4700,12 +4739,28 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                         <item row="8" column="2">
+                         <item row="4" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="2">
                           <widget class="QLabel" name="label_293">
                            <property name="text">
                             <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -4791,7 +4846,14 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="2">
+                         <item row="2" column="1">
+                          <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="11" column="2">
                           <widget class="QLabel" name="label_292">
                            <property name="text">
                             <string>Maximum laser scan points.</string>
@@ -4817,20 +4879,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="8" column="1">
-                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="1" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                            <property name="text">
@@ -4838,6 +4886,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="9" column="1">
+                          <widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="3" column="2">
                           <widget class="QLabel" name="label_251">
                            <property name="text">
@@ -4913,14 +4968,24 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </widget>
                          </item>
-                         <item row="8" column="0">
+                         <item row="9" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_path_scans">
                            <property name="text">
                             <string>...</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="1">
+                         <item row="11" column="1">
+                          <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="maximum">
+                            <number>99999999</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="1">
                           <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                            <property name="toolTip">
                             <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -4930,7 +4995,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="2">
+                         <item row="10" column="2">
                           <widget class="QLabel" name="label_294">
                            <property name="text">
                             <string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -4943,16 +5008,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="1">
-                          <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
-                           <property name="toolTip">
-                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-                           </property>
-                           <property name="maximum">
-                            <number>99999999</number>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="0" column="2">
                           <widget class="QLabel" name="label_265">
                            <property name="text">
@@ -5073,10 +5128,35 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="8" column="2">
-                          <widget class="QLineEdit" name="lineEdit_cameraImages_odom">
+                          <widget class="QLabel" name="label_443">
                            <property name="text">
-                            <string/>
+                            <string>Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="8" column="1">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_maxPoseTimeDiff">
+                           <property name="suffix">
+                            <string> s</string>
+                           </property>
+                           <property name="decimals">
+                            <number>4</number>
+                           </property>
+                           <property name="maximum">
+                            <double>9.990000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.020000000000000</double>
                            </property>
                           </widget>
                          </item>
@@ -6937,26 +7017,6 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
-                          <widget class="QLabel" name="label_101">
-                           <property name="text">
-                            <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="5" column="0">
-                          <widget class="QLineEdit" name="lineEdit_kp_roi">
-                           <property name="readOnly">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="0" column="0">
                           <widget class="QComboBox" name="comboBox_detector_strategy">
                            <item>
@@ -7011,6 +7071,26 @@ generate the number of words requested.</string>
                            </item>
                           </widget>
                          </item>
+                         <item row="5" column="0">
+                          <widget class="QLineEdit" name="lineEdit_kp_roi">
+                           <property name="readOnly">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
+                          <widget class="QLabel" name="label_101">
+                           <property name="text">
+                            <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_53">
                            <property name="toolTip">
@@ -7179,6 +7259,70 @@ generate the number of words requested.</string>
                            </property>
                           </widget>
                          </item>
+                         <item row="10" column="1">
+                          <widget class="QLabel" name="label_50">
+                           <property name="toolTip">
+                            <string>0 means that the response (hessian) threshold 
+used for the detector will not be adapted. 
+Otherwise, the threshold is modified to 
+generate the number of words requested.</string>
+                           </property>
+                           <property name="text">
+                            <string>Number of rows of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="0">
+                          <widget class="QSpinBox" name="spinBox_KPGridRows">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="11" column="0">
+                          <widget class="QSpinBox" name="spinBox_KPGridCols">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="11" column="1">
+                          <widget class="QLabel" name="label_86">
+                           <property name="toolTip">
+                            <string>0 means that the response (hessian) threshold 
+used for the detector will not be adapted. 
+Otherwise, the threshold is modified to 
+generate the number of words requested.</string>
+                           </property>
+                           <property name="text">
+                            <string>Number of columns of the grid used to extract uniformly &quot;max words / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -10296,7 +10440,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_odometryType">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>5</number>
                       </property>
                       <widget class="QWidget" name="page_52">
                        <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -12010,6 +12154,67 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
+                             <item row="3" column="2">
+                              <widget class="QLabel" name="label_444">
+                               <property name="text">
+                                <string>Camera FPS. This parameter is linked to &quot;Input rate&quot; of the Source panel.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="1">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_OdomORBSLAM2Fps">
+                               <property name="enabled">
+                                <bool>false</bool>
+                               </property>
+                               <property name="suffix">
+                                <string/>
+                               </property>
+                               <property name="decimals">
+                                <number>2</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.000000000000000</double>
+                               </property>
+                               <property name="maximum">
+                                <double>999.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.000000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="2">
+                              <widget class="QLabel" name="label_445">
+                               <property name="text">
+                                <string>Maximum ORB features extracted per frame.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="1">
+                              <widget class="QSpinBox" name="spinBox_OdomORBSLAM2MaxFeatures">
+                               <property name="maximum">
+                                <number>99999</number>
+                               </property>
+                               <property name="value">
+                                <number>1000</number>
+                               </property>
+                              </widget>
+                             </item>
                             </layout>
                            </item>
                           </layout>
@@ -12617,7 +12822,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item row="2" column="1">
                      <widget class="QLabel" name="label_370">
                       <property name="text">
-                       <string>Normalize covariance values. Position variances are multiplied by norm of the transform and orientation variances are multiplied by angle of the transform.</string>
+                       <string>Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration. It can be useful if the registration approach used can use a guess to get better matches.</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -12628,7 +12833,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      </widget>
                     </item>
                     <item row="2" column="0">
-                     <widget class="QCheckBox" name="reg_varianceNormalized">
+                     <widget class="QCheckBox" name="reg_repeatOnce">
                       <property name="text">
                        <string/>
                       </property>
@@ -12733,7 +12938,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="6" column="1">
+                      <item row="5" column="1">
                        <widget class="QLabel" name="label_298">
                         <property name="text">
                          <string>Forward estimation only (A-&gt;B). If false, a transformation is also computed in backward direction (B-&gt;A), then the two resulting transforms are merged (middle interpolation between the transforms).</string>
@@ -12746,15 +12951,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
-                       <widget class="QCheckBox" name="loopClosure_forwardEst">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="5" column="0">
-                       <widget class="QCheckBox" name="loopClosure_bowVarianceFromInliersCount">
+                       <widget class="QCheckBox" name="loopClosure_forwardEst">
                         <property name="text">
                          <string/>
                         </property>
@@ -12799,19 +12997,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_262">
-                        <property name="text">
-                         <string>Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="2" column="1">
                        <widget class="QLabel" name="label_2">
                         <property name="text">
@@ -12825,7 +13010,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="6" column="0">
                        <widget class="QComboBox" name="loopClosure_bundle">
                         <property name="sizeAdjustPolicy">
                          <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
@@ -12847,7 +13032,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </item>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="6" column="1">
                        <widget class="QLabel" name="label_346">
                         <property name="text">
                          <string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
@@ -12869,7 +13054,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                       <widget class="QWidget" name="page_54">
                        <layout class="QVBoxLayout" name="verticalLayout_85">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13009,7 +13203,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </widget>
                       <widget class="QWidget" name="page_55">
                        <layout class="QVBoxLayout" name="verticalLayout_86">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13167,7 +13370,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13247,7 +13459,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13359,7 +13580,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13619,6 +13849,58 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                            </property>
                           </widget>
                          </item>
+                         <item row="6" column="0">
+                          <widget class="QSpinBox" name="reextract_gridrows">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="6" column="1">
+                          <widget class="QLabel" name="label_438">
+                           <property name="text">
+                            <string>Number of rows of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="7" column="1">
+                          <widget class="QLabel" name="label_439">
+                           <property name="text">
+                            <string>Number of columns of the grid used to extract uniformly &quot;max features / grid cells&quot; features from each cell.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="7" column="0">
+                          <widget class="QSpinBox" name="reextract_gridcols">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>99</number>
+                           </property>
+                           <property name="value">
+                            <number>1</number>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -14014,7 +14296,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          <number>2</number>
                         </property>
                         <property name="maximum">
-                         <double>1.000000000000000</double>
+                         <double>10.000000000000000</double>
                         </property>
                         <property name="singleStep">
                          <double>0.010000000000000</double>
@@ -14812,10 +15094,18 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </widget>
                        </item>
                        <item row="5" column="0">
-                        <widget class="QDoubleSpinBox" name="stereo_minDisparity"/>
+                        <widget class="QDoubleSpinBox" name="stereo_minDisparity">
+                         <property name="maximum">
+                          <double>9999.000000000000000</double>
+                         </property>
+                        </widget>
                        </item>
                        <item row="6" column="0">
-                        <widget class="QDoubleSpinBox" name="stereo_maxDisparity"/>
+                        <widget class="QDoubleSpinBox" name="stereo_maxDisparity">
+                         <property name="maximum">
+                          <double>9999.000000000000000</double>
+                         </property>
+                        </widget>
                        </item>
                       </layout>
                      </widget>

tools/CMakeLists.txt

@@ -8,6 +8,7 @@ ADD_SUBDIRECTORY( StereoEval )
 ADD_SUBDIRECTORY( KittiDataset )
 ADD_SUBDIRECTORY( RgbdDataset )
 ADD_SUBDIRECTORY( Recovery )
+ADD_SUBDIRECTORY( Report )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/KittiDataset/main.cpp

@@ -60,19 +60,31 @@ void showUsage()
 			"  --scan             Include velodyne scan in node's data.\n"
 			"  --scan_step #      Scan downsample step (default=10).\n"
 			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
-			"  --scan_k           Scan normal K (default 20).\n"
+			"  --scan_k           Scan normal K (default 5).\n"
 			"  --scan_radius      Scan normal radius (default 0).\n"
 			"  --map_update  #    Do map update each X odometry frames (default=10, which\n"
 			"                        gives 1 Hz map update assuming images are at 10 Hz).\n\n"
 			"%s\n"
 			"Example:\n\n"
 			"   $ rtabmap-kitti_dataset \\\n"
-			"       --Vis/EstimationType 1\\\n"
 			"       --Vis/BundleAdjustment 1\\\n"
-			"       --Vis/PnPReprojError 1.5\\\n"
+			"       --Vis/PnPRefineIterations 0\\\n"
+			"       --Vis/MaxFeatures 1800\\\n"
+			"       --Vis/BundleAdjustment 1\\\n"
+			"       --Vis/Iterations 300\\\n"
+			"       --GFTT/QualityLevel 0.01\\\n"
+			"       --GFTT/MinDistance 7\\\n"
 			"       --Odom/GuessMotion true\\\n"
 			"       --OdomF2M/BundleAdjustment 1\\\n"
+			"       --Mem/UseOdomFeatures true\\\n"
+			"       --Kp/DetectorStrategy true\\\n"
+			"       --Kp/MaxFeatures 900\\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
 			"       --Rtabmap/CreateIntermediateNodes true\\\n"
+			"       --RGBD/ProximityBySpace false\\\n"
+			"       --Stereo/MaxLevel 5\\\n"
+			"       --Stereo/MaxDisparity 256\\\n"
+			"       --Stereo/MinDisparity 0.5\\\n"
 			"       --gt \"~/KITTI/devkit/cpp/data/odometry/poses/07.txt\"\\\n"
 			"       ~/KITTI/dataset/sequences/07\n\n", rtabmap::Parameters::showUsage());
 	exit(1);
@@ -105,7 +117,7 @@ int main(int argc, char * argv[])
 	bool disp = false;
 	int scanStep = 10;
 	float scanVoxel = 0.3f;
-	int scanNormalK = 20;
+	int scanNormalK = 5;
 	float scanNormalRadius = 0.0f;
 	std::string gtPath;
 	bool quiet = false;
@@ -377,7 +389,7 @@ int main(int argc, char * argv[])
 
 		printf("Processing %d images...\n", totalImages);
 
-		OdometryF2M odom(parameters);
+		Odometry * odom = Odometry::create(parameters);
 		Rtabmap rtabmap;
 		rtabmap.init(parameters, databasePath);
 
@@ -408,7 +420,7 @@ int main(int argc, char * argv[])
 			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
 
 			OdometryInfo odomInfo;
-			Transform pose = odom.process(data, &odomInfo);
+			Transform pose = odom->process(data, &odomInfo);
 			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
 			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
 			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
@@ -433,14 +445,10 @@ int main(int argc, char * argv[])
 				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 				processData = intermediateNodes;
 			}
-			if(covariance.empty())
+			if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
 			{
 				covariance = odomInfo.reg.covariance;
 			}
-			else
-			{
-				covariance += odomInfo.reg.covariance;
-			}
 
 			timer.restart();
 			if(processData)
@@ -463,8 +471,8 @@ int main(int argc, char * argv[])
 
 				if(rmse >= 0.0f)
 				{
-					printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm",
+					printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
-							iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+							iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
 				}
 				else
 				{
@@ -487,13 +495,14 @@ int main(int argc, char * argv[])
 			timer.restart();
 			data = cameraThread.camera()->takeImage(&cameraInfo);
 		}
+		delete odom;
 		printf("Total time=%fs\n", totalTime.ticks());
 		/////////////////////////////
 		// Processing dataset end
 		/////////////////////////////
 
 		// Save trajectory
-		printf("Saving rtabmap_trajectory.txt ...\n");
+		printf("Saving trajectory ...\n");
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
 		rtabmap.getGraph(poses, links, true, true);
@@ -575,6 +584,8 @@ int main(int argc, char * argv[])
 				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
 			}
 			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  KITTI t_err =         %f %%\n", t_err);
+			fprintf(pFile, "  KITTI r_err =         %f deg/m\n", r_err);
 			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
 			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
 			fprintf(pFile, "  translational_median= %f\n", translational_median);

tools/Report/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 2.8)
+
+# inside rtabmap project (see below for external build)
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+	
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(report main.cpp)
+  
+TARGET_LINK_LIBRARIES(report ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( report 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-report)

tools/Report/main.cpp

@@ -0,0 +1,142 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-report path\n"
+			"  path               Directory containing rtabmap databases.\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	std::string path = argv[1];
+	path = uReplaceChar(path, '~', UDirectory::homeDir());
+
+	std::string fileName;
+	std::list<std::string> paths;
+	paths.push_back(path);
+	while(paths.size())
+	{
+		std::string currentPath = paths.front();
+		UDirectory currentDir(currentPath);
+		paths.pop_front();
+		if(!currentDir.isValid())
+		{
+			continue;
+		}
+		std::list<std::string> subDirs;
+		printf("Directory: %s\n", currentPath.c_str());
+		while(!(fileName = currentDir.getNextFileName()).empty())
+		{
+			if(UFile::getExtension(fileName).compare("db") == 0)
+			{
+				std::string filePath = currentPath + UDirectory::separator() + fileName;
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(filePath))
+				{
+					std::set<int> ids;
+					driver->getAllNodeIds(ids);
+					std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
+					std::vector<float> cameraTime;
+					cameraTime.reserve(ids.size());
+					std::vector<float> odomTime;
+					odomTime.reserve(ids.size());
+					std::vector<float> slamTime;
+					slamTime.reserve(ids.size());
+					float rmse = -1;
+					for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+					{
+						Transform p, gt;
+						GPS gps;
+						int m, w;
+						std::string l;
+						double s;
+						std::vector<float> v;
+						driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps);
+
+						if(uContains(stats, *iter))
+						{
+							const std::map<std::string, float> & stat = stats.at(*iter).first;
+							if(uContains(stat, Statistics::kGtTranslational_rmse()))
+							{
+								rmse = stat.at(Statistics::kGtTranslational_rmse());
+							}
+							if(uContains(stat, std::string("Camera/TotalTime/ms")))
+							{
+								cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
+							}
+							if(uContains(stat, std::string("Odometry/TotalTime/ms")))
+							{
+								odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
+							}
+							if(w >= 0 && uContains(stat, Statistics::kTimingTotal()))
+							{
+								slamTime.push_back(stat.at(Statistics::kTimingTotal()));
+							}
+						}
+					}
+					printf("   %s (%d): %fm, slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
+							fileName.c_str(),
+							(int)ids.size(),
+							rmse,
+							(int)uMean(slamTime), (int)uMax(slamTime),
+							(int)uMean(odomTime), (int)uMax(odomTime),
+							(int)uMean(cameraTime), (int)uMax(cameraTime));
+				}
+				driver->closeConnection();
+				delete driver;
+			}
+			else if(uSplit(fileName, '.').size() == 1)
+			{
+				//sub directory
+				subDirs.push_front(currentPath + UDirectory::separator() + fileName);
+			}
+		}
+		for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
+		{
+			paths.push_front(*iter);
+		}
+	}
+
+	return 0;
+}

tools/RgbdDataset/main.cpp

@@ -59,13 +59,20 @@ void showUsage()
 			"%s\n"
 			"Example:\n\n"
 			"   $ rtabmap-rgbd_dataset \\\n"
-			"       --Vis/EstimationType 1\\\n"
 			"       --Vis/BundleAdjustment 1\\\n"
-			"       --Vis/PnPReprojError 1.5\\\n"
+			"       --Vis/PnPRefineIterations 0\\\n"
+			"       --Vis/BundleAdjustment 1\\\n"
+			"       --Vis/Iterations 300\\\n"
+			"       --GFTT/QualityLevel 0.001\\\n"
+			"       --GFTT/MinDistance 3\\\n"
 			"       --Odom/GuessMotion true\\\n"
 			"       --OdomF2M/BundleAdjustment 1\\\n"
+			"       --Mem/UseOdomFeatures true\\\n"
+			"       --Kp/DetectorStrategy true\\\n"
+			"       --Kp/MaxFeatures 600\\\n"
+			"       --Rtabmap/DetectionRate 4\\\n"
 			"       --Rtabmap/CreateIntermediateNodes true\\\n"
-			"       --Rtabmap/DetectionRate 1\\\n"
+			"       --RGBD/ProximityBySpace false\\\n"
 			"       ~/rgbd_dataset_freiburg3_long_office_household\n\n", rtabmap::Parameters::showUsage());
 	exit(1);
 }
@@ -123,6 +130,7 @@ int main(int argc, char * argv[])
 		}
 	}
 
+	std::string seq = uSplit(path, '/').back();
 	std::string pathRgbImages  = path+"/rgb_sync";
 	std::string pathDepthImages = path+"/depth_sync";
 	std::string pathGt = path+"/groundtruth.txt";
@@ -138,10 +146,12 @@ int main(int argc, char * argv[])
 	}
 
 	printf("Paths:\n"
+			"   Dataset name:    %s\n"
 			"   Dataset path:    %s\n"
 			"   RGB path:        %s\n"
 			"   Depth path:      %s\n"
 			"   Output:          %s\n",
+			seq.c_str(),
 			path.c_str(),
 			pathRgbImages.c_str(),
 			pathDepthImages.c_str(),
@@ -172,13 +182,14 @@ int main(int argc, char * argv[])
 	else if(sequenceName.find("freiburg2") != std::string::npos)
 	{
 		model = CameraModel("rtabmap_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
-		depthFactor = 5.208f;
+		depthFactor = 5.208f; // based on TUM2.yaml ORB_SLAM2 file
 	}
 	else //if(sequenceName.find("freiburg3") != std::string::npos)
 	{
 		model = CameraModel("rtabmap_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
 	}
-	model.save(output);
+	//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
+	model.save(path);
 
 	CameraThread cameraThread(new
 		CameraRGBDImages(
@@ -197,15 +208,15 @@ int main(int argc, char * argv[])
 	float detectionRate = Parameters::defaultRtabmapDetectionRate();
 	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
 	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
-	std::string databasePath = output+"/rtabmap.db";
+	std::string databasePath = output+"/"+seq+".db";
 	UFile::erase(databasePath);
-	if(cameraThread.camera()->init(output, "rtabmap_calib"))
+	if(cameraThread.camera()->init(path, "rtabmap_calib"))
 	{
 		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
 
 		printf("Processing %d images...\n", totalImages);
 
-		OdometryF2M odom(parameters);
+		Odometry * odom = Odometry::create(parameters);
 		Rtabmap rtabmap;
 		rtabmap.init(parameters, databasePath);
 
@@ -237,11 +248,18 @@ int main(int argc, char * argv[])
 			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
 
 			OdometryInfo odomInfo;
-			Transform pose = odom.process(data, &odomInfo);
+			Transform pose = odom->process(data, &odomInfo);
 			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
 			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
 			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
 			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+			externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+			externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
 
 			bool processData = true;
 			if(detectionRate>0.0f &&
@@ -263,14 +281,10 @@ int main(int argc, char * argv[])
 				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 				processData = intermediateNodes;
 			}
-			if(covariance.empty())
+			if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
 			{
 				covariance = odomInfo.reg.covariance;
 			}
-			else
-			{
-				covariance += odomInfo.reg.covariance;
-			}
 
 			timer.restart();
 			if(processData)
@@ -293,8 +307,8 @@ int main(int argc, char * argv[])
 
 				if(rmse >= 0.0f)
 				{
-					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm",
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms, rmse=%fm, stddev=%fm %frad",
-							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
 				}
 				else
 				{
@@ -317,18 +331,25 @@ int main(int argc, char * argv[])
 			timer.restart();
 			data = cameraThread.camera()->takeImage(&cameraInfo);
 		}
+		delete odom;
 		printf("Total time=%fs\n", totalTime.ticks());
 		/////////////////////////////
 		// Processing dataset end
 		/////////////////////////////
 
 		// Save trajectory
-		printf("Saving rtabmap_trajectory.txt ...\n");
+		printf("Saving trajectory...\n");
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
-		rtabmap.getGraph(poses, links, true, true);
+		std::map<int, Signature> signatures;
-		std::string pathTrajectory = output+"/rtabmap_poses.txt";
+		std::map<int, double> stamps;
-		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+seq+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
 		{
 			printf("Saving %s... done!\n", pathTrajectory.c_str());
 		}
@@ -357,13 +378,6 @@ int main(int argc, char * argv[])
 				}
 			}
 
-			// compute KITTI statistics
-			float t_err = 0.0f;
-			float r_err = 0.0f;
-			graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
-			printf("Ground truth comparison:\n");
-			printf("   KITTI t_err = %f %%\n", t_err);
-			printf("   KITTI r_err = %f deg/m\n", r_err);
 
 			// compute RMSE statistics
 			float translational_rmse = 0.0f;
@@ -398,7 +412,7 @@ int main(int argc, char * argv[])
 			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
 
 			FILE * pFile = 0;
-			std::string pathErrors = output+"/rtabmap_rmse.txt";
+			std::string pathErrors = output+"/"+seq+"_rmse.txt";
 			pFile = fopen(pathErrors.c_str(),"w");
 			if(!pFile)
 			{
@@ -425,9 +439,9 @@ int main(int argc, char * argv[])
 		UERROR("Camera init failed!");
 	}
 
-	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap.db").c_str());
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+seq+".db").c_str());
 	printf("Do:\n"
-			" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap.db").c_str());
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+seq+".db").c_str());
 
 	return 0;
 }