0.11.13. OdometryF2M: fixed/improved bundle adjustment option. OptimizerG2O: now doing stereo BA instead of mono BA (interface of Optimizer::optimzeBA() has slightly changed to include depth in word references). Registration and Odometry set angular and linear variances separately. RegistrationVis set x100 smaller variance for rotation. Added keypoints3D member to SensorData. Added Transform::getAngle() for convenience. Odometry normalizes variances. RtabmapThread does variance summation when frames are discarded. Updated how variance is computed by util3d::estimateMotion3Dto2D(), ignoring very large variance from the computation (stereo issue with very far matched features). MainWindow: added option to align with ground truth or not (default true as before), added for odometry statistics (including bundle stuff), update ground truth statistics every time a graph is loaded/optimized. CameraRGB can now load odometry files (to fake an input odometry). PreferencesDialog: Added option to Odometry so it can use a different registration approach than the default one (the one used for loop closure). Parameters: added new g2o/Solver option 3 (Eigen), added g2o/RobustKernelDelta, g2o/Baseline and Odom/VisKeyFrameThr. Database: from 0.11.13, ignoring patch version when comparing is database version is newer than app version used.

CMakeLists.txt

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

corelib/include/rtabmap/core/CameraRGB.h

@@ -59,6 +59,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return odometry_.size() > 0; }
 	std::string getPath() const {return _path;}
 	unsigned int imagesCount() const;
 	std::vector<std::string> filenames() const;
@@ -75,8 +76,8 @@ public:
 	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
 	{
 		_filenamesAreTimestamps = fileNamesAreStamps;
-		timestampsPath_=filePath;
+		_timestampsPath=filePath;
-		syncImageRateWithStamps_ = syncImageRateWithStamps;
+		_syncImageRateWithStamps = syncImageRateWithStamps;
 	}
 
 	void setScanPath(
@@ -106,9 +107,15 @@ public:
 		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
 	}
 
+	void setOdometryPath(const std::string & filePath, int format = 0)
+	{
+		_odometryPath = filePath;
+		_odometryFormat = format;
+	}
+
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
-		groundTruthPath_ = filePath;
+		_groundTruthPath = filePath;
 		_groundTruthFormat = format;
 	}
 
@@ -120,6 +127,7 @@ public:
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
+	bool readPoses(std::list<Transform> & outputPoses, std::list<double> & stamps, const std::string & filePath, int format) const;
 
 private:
 	std::string _path;
@@ -150,13 +158,17 @@ private:
 	bool _depthFromScanFillHolesFromBorder;
 
 	bool _filenamesAreTimestamps;
-	std::string timestampsPath_;
+	std::string _timestampsPath;
-	bool syncImageRateWithStamps_;
+	bool _syncImageRateWithStamps;
 
-	std::string groundTruthPath_;
+	std::string _odometryPath;
+	int _odometryFormat;
+
+	std::string _groundTruthPath;
 	int _groundTruthFormat;
 
-	std::list<double> stamps_;
+	std::list<double> _stamps;
+	std::list<Transform> odometry_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
 

corelib/include/rtabmap/core/Features2d.h

@@ -133,6 +133,7 @@ public:
 
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

corelib/include/rtabmap/core/OdometryF2F.h

@@ -53,6 +53,7 @@ private:
 private:
 	//Parameters:
 	float keyFrameThr_;
+	int visKeyFrameThr_;
 	float scanKeyFrameThr_;
 
 	Registration * registrationPipeline_;

corelib/include/rtabmap/core/OdometryF2M.h

@@ -38,6 +38,7 @@ namespace rtabmap {
 
 class Signature;
 class Registration;
+class Optimizer;
 
 class RTABMAP_EXP OdometryF2M : public Odometry
 {
@@ -58,24 +59,25 @@ private:
 	//Parameters
 	int maximumMapSize_;
 	float keyFrameThr_;
+	int visKeyFrameThr_;
 	int maxNewFeatures_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
 	int bundleAdjustment_;
-	int	bundleAdjustmentMaxFrames_;
+	int bundleMaxFrames_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 
-	std::map<int, std::map<int, cv::Point2f> > bundleWordReferences_;
+	std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;
 	std::multimap<int, Link> bundleLinks_;
 	std::map<int, CameraModel> bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
-	ParametersMap bundleParameters_;
+	Optimizer * sba_;
 };
 
 }

corelib/include/rtabmap/core/OdometryInfo.h

@@ -42,10 +42,16 @@ public:
 		matches(0),
 		inliers(0),
 		icpInliersRatio(0.0f),
-		variance(0.0f),
+		varianceLin(0.0f),
+		varianceAng(0.0f),
 		features(0),
 		localMapSize(0),
 		localScanMapSize(0),
+		localKeyFrames(0),
+		localBundleOutliers(0),
+		localBundleConstraints(0),
+		localBundleTime(0),
+		keyFrameAdded(false),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -61,10 +67,16 @@ public:
 		output.matches = matches;
 		output.inliers = inliers;
 		output.icpInliersRatio = icpInliersRatio;
-		output.variance = variance;
+		output.varianceLin = varianceLin;
+		output.varianceAng = varianceAng;
 		output.features = features;
 		output.localMapSize = localMapSize;
 		output.localScanMapSize = localScanMapSize;
+		output.localKeyFrames = localKeyFrames;
+		output.localBundleOutliers = localBundleOutliers;
+		output.localBundleConstraints = localBundleConstraints;
+		output.localBundleTime = localBundleTime;
+		output.keyFrameAdded = keyFrameAdded;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -80,10 +92,16 @@ public:
 	int matches;
 	int inliers;
 	float icpInliersRatio;
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	int features;
 	int localMapSize;
 	int localScanMapSize;
+	int localKeyFrames;
+	int localBundleOutliers;
+	int localBundleConstraints;
+	float localBundleTime;
+	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;

corelib/include/rtabmap/core/Optimizer.h

@@ -94,12 +94,13 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
-			int rootId,
+			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models,
+			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			std::set<int> * outliers = 0);
 
 	std::map<int, Transform> optimizeBA(
 			int rootId,
@@ -111,14 +112,15 @@ public:
 			const Link & link,
 			const CameraModel & model,
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point2f> > & wordReferences);
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+			std::set<int> * outliers = 0);
 
 	void computeBACorrespondences(
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures,
 			std::map<int, cv::Point3f> & points3DMap,
-			std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
+			std::map<int, std::map<int, cv::Point3f> > & wordReferences); // <ID words, IDs frames + keypoint/depth>
 
 protected:
 	Optimizer(

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -57,7 +57,8 @@ public:
 			const std::multimap<int, Link> & links,
 			const std::map<int, CameraModel> & models,
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			std::set<int> * outliers = 0);
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -51,7 +51,9 @@ public:
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer()),
-        pixelVariance_(Parameters::defaultg2oPixelVariance())
+        pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
+		baseline_(Parameters::defaultg2oBaseline())
 	{
 		parseParameters(parameters);
 	}
@@ -73,14 +75,17 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models,
+			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
-			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
+			const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
+			std::set<int> * outliers = 0);
 
 private:
 	int solver_;
 	int optimizer_;
 	double pixelVariance_;
+	double robustKernelDelta_;
+	double baseline_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -269,7 +269,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, GridRows,           int, 4,         "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 	RTABMAP_PARAM(FAST, GridCols,           int, 4,         "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 
-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
@@ -336,23 +336,27 @@ class RTABMAP_EXP Parameters
 	// Graph optimization
 #ifdef RTABMAP_GTSAM
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
-	RTABMAP_PARAM(Optimizer, Iterations,        int, 10,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 20,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001,  "Stop optimizing when the error improvement is less than this value.");
 #else
 #ifdef RTABMAP_G2O
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
-	RTABMAP_PARAM(Optimizer, Iterations,        int, 10,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 20,          "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0,     "Stop optimizing when the error improvement is less than this value.");
 #else
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
 	RTABMAP_PARAM(Optimizer, Iterations,        int, 100,        "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
 #endif
 #endif
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
 	RTABMAP_PARAM(Optimizer, Robust,            bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
 
-	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
+	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
 	RTABMAP_PARAM(g2o, PixelVariance,           double, 1.0,     "Pixel variance used for bundle adjustment.");
+	RTABMAP_PARAM(g2o, RobustKernelDelta,       double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
+	RTABMAP_PARAM(g2o, Baseline,                double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -370,6 +374,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
 	RTABMAP_PARAM(Odom, GuessMotion,            bool, false,      "Guess next transformation from the last motion computed.");
 	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+	RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 100,         "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,           "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
 	RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,      "Align odometry with the ground on initialization.");
@@ -399,7 +404,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
+	RTABMAP_PARAM(Vis, PnPFlags,                 int, 0,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
@@ -424,8 +429,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02,   "See cv::cornerSubPix().");
 	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.8,    uFormat("[%s=0] NNDR: nearest neighbor distance ratio. 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()));
-	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 50,       uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 20,       uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
 	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
@@ -457,7 +462,7 @@ class RTABMAP_EXP Parameters
 
 	RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
-	RTABMAP_PARAM(StereoBM, NumDisparities,      int, 64,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, NumDisparities,      int, 128,      "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, PreFilterSize,       int, 9,        "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, PreFilterCap,        int, 31,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, UniquenessRatio,     int, 15,       "See cv::StereoBM");

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -35,7 +35,8 @@ class RegistrationInfo
 {
 public:
 	RegistrationInfo() :
-		variance(0),
+		varianceLin(0),
+		varianceAng(0),
 		inliers(0),
 		matches(0),
 		icpInliersRatio(0),
@@ -44,7 +45,8 @@ public:
 	{
 	}
 
-	float variance;
+	float varianceLin;
+	float varianceAng;
 	std::string rejectedMsg;
 
 	// RegistrationVis

corelib/include/rtabmap/core/SensorData.h

@@ -213,12 +213,9 @@ public:
 	float gridCellSize() const {return _cellSize;}
 	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
 
-	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
+	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors);
-	{
-		_keypoints = keypoints;
-		_descriptors = descriptors;
-	}
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
@@ -257,6 +254,7 @@ private:
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
+	std::vector<cv::Point3f> _keypoints3D;
 	cv::Mat _descriptors;
 
 	Transform groundTruth_;

corelib/include/rtabmap/core/Statistics.h

@@ -94,6 +94,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
 	RTABMAP_STATS(Memory, Local_graph_size,);
 	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Odometry_variance_ang,);
+	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);

corelib/include/rtabmap/core/Transform.h

@@ -106,6 +106,7 @@ public:
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
+	float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
 	float getNorm() const;
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;

corelib/src/CameraRGB.cpp

@@ -74,7 +74,8 @@ CameraImages::CameraImages() :
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
 		_filenamesAreTimestamps(false),
-		syncImageRateWithStamps_(true),
+		_syncImageRateWithStamps(true),
+		_odometryFormat(0),
 		_groundTruthFormat(0),
 		_captureDelay(0.0)
 	{}
@@ -102,7 +103,8 @@ CameraImages::CameraImages(const std::string & path,
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
 	_filenamesAreTimestamps(false),
-	syncImageRateWithStamps_(true),
+	_syncImageRateWithStamps(true),
+	_odometryFormat(0),
 	_groundTruthFormat(0),
 	_captureDelay(0.0)
 {
@@ -226,7 +228,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	}
 
 	bool success = _dir->isValid();
-	stamps_.clear();
+	_stamps.clear();
+	odometry_.clear();
 	groundTruth_.clear();
 	if(success)
 	{
@@ -246,7 +249,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					double stamp = uStr2Double(sec + "." + decimals);
 					if(stamp > 0.0)
 					{
-						stamps_.push_back(stamp);
+						_stamps.push_back(stamp);
 					}
 					else
 					{
@@ -254,19 +257,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 				}
 			}
-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count is not the same as the images (%d vs %d)! "
 					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), this->imagesCount());
+						(int)_stamps.size(), this->imagesCount());
-				stamps_.clear();
+				_stamps.clear();
 				success = false;
 			}
 		}
-		else if(timestampsPath_.size())
+		else if(_timestampsPath.size())
 		{
 			std::ifstream file;
-			file.open(timestampsPath_.c_str(), std::ifstream::in);
+			file.open(_timestampsPath.c_str(), std::ifstream::in);
 			while(file.good())
 			{
 				std::string str;
@@ -290,120 +293,29 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 					stampStr = stampStr+'.'+millisecStr;
 				}
-				stamps_.push_back(uStr2Double(stampStr));
+				_stamps.push_back(uStr2Double(stampStr));
 			}
 
 			file.close();
 
-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
 						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
+						(int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
-				stamps_.clear();
+				_stamps.clear();
 				success = false;
 			}
 		}
 
-		if(groundTruthPath_.size())
+		if(success && _odometryPath.size())
 		{
-			std::map<int, Transform> poses;
+			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
-			std::map<int, double> stamps;
+		}
-			if(!graph::importPoses(groundTruthPath_, _groundTruthFormat, poses, 0, &stamps))
-			{
-				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5 && _groundTruthFormat != 6 && _groundTruthFormat != 7) && poses.size() != this->imagesCount())
-			{
-				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
-						"the ground truth file path if you don't want to use it (current file path=%s).",
-						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7) && stamps_.size() == 0)
-			{
-				UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats for ground truth, images must have timestamps!");
-				success = false;
-			}
-			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7)
-			{
-				UDEBUG("");
-				//Match ground truth values with images
-				groundTruth_.clear();
-				std::map<double, int> stampsToIds;
-				for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
-				{
-					stampsToIds.insert(std::make_pair(iter->second, iter->first));
-				}
-				std::vector<double> values = uValues(stamps);
 
-				int validPoses = 0;
+		if(success && _groundTruthPath.size())
-				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
+		{
-				{
+			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
-					Transform pose; // null transform
-					std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
-					bool warned = false;
-					if(endIter != stampsToIds.end())
-					{
-						if(endIter->first == *ster)
-						{
-							pose = poses.at(endIter->second);
-						}
-						else if(endIter != stampsToIds.begin())
-						{
-							//interpolate
-							std::map<double, int>::iterator beginIter = endIter;
-							--beginIter;
-							double stampBeg = beginIter->first;
-							double stampEnd = endIter->first;
-							UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-							if(stampEnd - stampBeg > 10.0)
-							{
-								warned = true;
-								UDEBUG("Cannot interpolate ground truth pose for stamp %f between %f and %f (>10 sec)", 
-									*ster,
-									stampBeg,
-									stampEnd);
-							}
-							else
-							{
-								float t = (*ster - stampBeg) / (stampEnd-stampBeg);
-								Transform & ta = poses.at(beginIter->second);
-								Transform & tb = poses.at(endIter->second);
-								if(!ta.isNull() && !tb.isNull())
-								{
-									++validPoses;
-									pose = ta.interpolate(t, tb);
-								}
-							}
-						}
-					}
-					if(pose.isNull() && !warned)
-					{
-						UDEBUG("Ground truth pose not found for stamp %f", *ster);
-					}
-					groundTruth_.push_back(pose);
-				}
-				if(validPoses != (int)stamps_.size())
-				{
-					UWARN("%d valid ground truth poses of %d stamps", validPoses, (int)stamps_.size());
-				}
-			}
-			else
-			{
-				UDEBUG("");
-				groundTruth_ = uValuesList(poses);
-				if(stamps_.size() == 0 && stamps.size() == poses.size())
-				{
-					stamps_ = uValuesList(stamps);
-				}
-				else if(_groundTruthFormat==8 && stamps_.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
-				{
-					UERROR("With Karlsruhe ground truth format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
-				}
-			}
-			UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
 		}
 	}
 
@@ -412,6 +324,110 @@ 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
+{
+	outputPoses.clear();
+	std::map<int, Transform> poses;
+	std::map<int, double> stamps;
+	if(!graph::importPoses(filePath, format, poses, 0, &stamps))
+	{
+		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
+		return false;
+	}
+	else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
+	{
+		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
+				"the pose file path if you don't want to use it (current file path=%s).",
+				(int)poses.size(), this->imagesCount(), filePath.c_str());
+		return false;
+	}
+	else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
+	{
+		UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
+		return false;
+	}
+	else if(format == 1 || format == 5 || format == 6 || format == 7)
+	{
+		UDEBUG("");
+		//Match ground truth values with images
+		outputPoses.clear();
+		std::map<double, int> stampsToIds;
+		for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
+		{
+			stampsToIds.insert(std::make_pair(iter->second, iter->first));
+		}
+		std::vector<double> values = uValues(stamps);
+
+		int validPoses = 0;
+		for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
+		{
+			Transform pose; // null transform
+			std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
+			bool warned = false;
+			if(endIter != stampsToIds.end())
+			{
+				if(endIter->first == *ster)
+				{
+					pose = poses.at(endIter->second);
+				}
+				else if(endIter != stampsToIds.begin())
+				{
+					//interpolate
+					std::map<double, int>::iterator beginIter = endIter;
+					--beginIter;
+					double stampBeg = beginIter->first;
+					double stampEnd = endIter->first;
+					UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
+					if(stampEnd - stampBeg > 10.0)
+					{
+						warned = true;
+						UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
+							*ster,
+							stampBeg,
+							stampEnd);
+					}
+					else
+					{
+						float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+						Transform & ta = poses.at(beginIter->second);
+						Transform & tb = poses.at(endIter->second);
+						if(!ta.isNull() && !tb.isNull())
+						{
+							++validPoses;
+							pose = ta.interpolate(t, tb);
+						}
+					}
+				}
+			}
+			if(pose.isNull() && !warned)
+			{
+				UDEBUG("Pose not found for stamp %f", *ster);
+			}
+			outputPoses.push_back(pose);
+		}
+		if(validPoses != (int)inOutStamps.size())
+		{
+			UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
+		}
+	}
+	else
+	{
+		UDEBUG("");
+		outputPoses = uValuesList(poses);
+		if(inOutStamps.size() == 0 && stamps.size() == poses.size())
+		{
+			inOutStamps = uValuesList(stamps);
+		}
+		else if(format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
+		{
+			UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
+			return false;
+		}
+	}
+	UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
+	return true;
+}
+
 bool CameraImages::isCalibrated() const
 {
 	return _model.isValidForProjection();
@@ -442,7 +458,7 @@ std::vector<std::string> CameraImages::filenames() const
 
 SensorData CameraImages::captureImage(CameraInfo * info)
 {
-	if(syncImageRateWithStamps_ && _captureDelay>0.0)
+	if(_syncImageRateWithStamps && _captureDelay>0.0)
 	{
 		int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime());
 		if(sleepTime > 2)
@@ -476,6 +492,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	cv::Mat img;
 	cv::Mat scan;
 	double stamp = UTimer::now();
+	Transform odometryPose;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
 	UDEBUG("");
@@ -541,13 +558,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}
 
-		if(stamps_.size())
+		if(_stamps.size())
 		{
-			stamp = stamps_.front();
+			stamp = _stamps.front();
-			stamps_.pop_front();
+			_stamps.pop_front();
-			if(stamps_.size())
+			if(_stamps.size())
 			{
-				_captureDelay = stamps_.front() - stamp;
+				_captureDelay = _stamps.front() - stamp;
+			}
+			if(odometry_.size())
+			{
+				odometryPose = odometry_.front();
+				odometry_.pop_front();
 			}
 			if(groundTruth_.size())
 			{
@@ -688,6 +710,13 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 
 	SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
+
+	if(info && !odometryPose.isNull())
+	{
+		info->odomPose = odometryPose;
+		info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
+	}
+
 	return data;
 }
 

corelib/src/CameraThread.cpp

@@ -228,7 +228,15 @@ void CameraThread::mainLoop()
 					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
 					data.stereoCameraModel().left().fx(),
 					data.stereoCameraModel().baseline());
-			data.setCameraModel(data.stereoCameraModel().left());
+			// set Tx for stereo bundle adjustment (when used)
+			CameraModel model = CameraModel(
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().left().fy(),
+					data.stereoCameraModel().left().cx(),
+					data.stereoCameraModel().left().cy(),
+					data.stereoCameraModel().localTransform(),
+					-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+			data.setCameraModel(model);
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
 			info.timeDisparity = timer.ticks();

corelib/src/DBDriverSqlite3.cpp

@@ -307,7 +307,6 @@ bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
 	return false;
 }
 
-
 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
 {
 	this->disconnectDatabaseQuery();
@@ -381,13 +380,15 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	UASSERT(this->getDatabaseVersionQuery(_version)); // must be true!
 	UINFO("Database version = %s", _version.c_str());
 
-	if(uStrNumCmp(_version, RTABMAP_VERSION) > 0)
+	// From 0.11.13, compare only with minor version (patch will be used for non-database structural changes)
+	if((uStrNumCmp(_version, "0.11.12") <= 0 && uStrNumCmp(_version, RTABMAP_VERSION) > 0) ||
+	   (uStrNumCmp(_version, "0.11.12") > 0 && uStrNumCmp(RTABMAP_VERSION, "0.11.12") > 0 && uStrNumCmp(_version, uFormat("%d.%d.99", RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR)) > 0))
 	{
-		UERROR("Opened database version (%s) is more recent than rtabmap "
+			UERROR("Opened database version (%s) is more recent than rtabmap "
-			   "installed version (%s). Please update rtabmap to new version!",
+				   "installed version (%s). Please update rtabmap to new version!",
-			   _version.c_str(), RTABMAP_VERSION);
+				   _version.c_str(), RTABMAP_VERSION);
-		this->disconnectDatabaseQuery(false);
+			this->disconnectDatabaseQuery(false);
-		return false;
+			return false;
 	}
 
 	//Set database optimizations

corelib/src/Features2d.cpp

@@ -209,8 +209,15 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKey
 }
 
 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+{
+	std::vector<cv::Point3f> keypoints3D;
+	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
+}
+
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
 {
 	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
+	UASSERT_MSG((int)keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
@@ -229,6 +236,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		int removed = (int)hessianMap.size()-maxKeypoints;
 		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
 		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		std::vector<cv::Point3f> kpts3DTmp(maxKeypoints);
 		cv::Mat descriptorsTmp;
 		if(descriptors.rows)
 		{
@@ -237,6 +245,10 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
 		{
 			kptsTmp[k] = keypoints[iter->second];
+			if(keypoints3D.size())
+			{
+				kpts3DTmp[k] = keypoints3D[iter->second];
+			}
 			if(descriptors.rows)
 			{
 				if(descriptors.type() == CV_32FC1)
@@ -252,6 +264,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
 		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
 		keypoints = kptsTmp;
+		keypoints3D = kpts3DTmp;
 		if(descriptors.rows)
 		{
 			descriptors = descriptorsTmp;

corelib/src/Memory.cpp

@@ -2187,11 +2187,11 @@ Transform Memory::computeTransform(
 			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
 			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
 			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
 			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 		else if(useKnownCorrespondencesIfPossible)
 		{
@@ -3317,14 +3317,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	{
 		UINFO("Use odometry features");
 		keypoints = data.keypoints();
+		keypoints3D = data.keypoints3D();
 		descriptors = data.descriptors().clone();
 
 		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
+		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
 
 		if((int)keypoints.size() > _feature2D->getMaxFeatures())
 		{
-			_feature2D->limitKeypoints(keypoints, descriptors, _feature2D->getMaxFeatures());
+			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, _feature2D->getMaxFeatures());
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+		UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 		if(descriptors.empty())
 		{
@@ -3339,13 +3344,14 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			}
 
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+		UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
-		if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+		if(keypoints3D.empty() &&
-		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+			((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
 		{
 			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
 			if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
@@ -3376,10 +3382,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				keypoints3D = validKeypoints3D;
 				descriptors = validDescriptors.rowRange(0, oi).clone();
 			}
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+		UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 
 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -3500,7 +3506,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
 
 	// apply decimation?
-	if(_imagePostDecimation > 1)
+	if(_imagePostDecimation > 1 && !isIntermediateNode)
 	{
 		image = util2d::decimate(image, _imagePostDecimation);
 		depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
@@ -3521,7 +3527,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	// downsampling the laser scan?
 	cv::Mat laserScan = data.laserScanRaw();
 	int maxLaserScanMaxPts = data.laserScanInfo().maxPoints();
-	if(!laserScan.empty() && _laserScanDownsampleStepSize > 1)
+	if(!laserScan.empty() && _laserScanDownsampleStepSize > 1 && !isIntermediateNode)
 	{
 		laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
 		maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
@@ -3530,7 +3536,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
-	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3)
+	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
 		float x,y,z;
@@ -3543,7 +3549,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	Signature * s;
-	if(this->isBinDataKept())
+	if(this->isBinDataKept() && !isIntermediateNode)
 	{
 		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
@@ -3564,10 +3570,22 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
 		rtabmap::CompressionThread ctLaserScan(laserScan);
 		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		ctImage.start();
+		if(!image.empty())
-		ctDepth.start();
+		{
-		ctLaserScan.start();
+			ctImage.start();
-		ctUserData.start();
+		}
+		if(!depthOrRightImage.empty())
+		{
+			ctDepth.start();
+		}
+		if(!laserScan.empty())
+		{
+			ctLaserScan.start();
+		}
+		if(!data.userDataRaw().empty())
+		{
+			ctUserData.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
@@ -3602,11 +3620,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else
 	{
+		UDEBUG("Bin data kept: scan=%d, userData=%d",
+						laserScan.empty()?0:1,
+						data.userDataRaw().empty()?0:1);
+
 		// just compress user data and laser scan (scans can be used for local scan matching)
 		rtabmap::CompressionThread ctUserData(data.userDataRaw());
 		rtabmap::CompressionThread ctLaserScan(laserScan);
-		ctUserData.start();
+		if(!data.userDataRaw().empty() && !isIntermediateNode)
-		ctLaserScan.start();
+		{
+			ctUserData.start();
+		}
+		if(!laserScan.empty() && !isIntermediateNode)
+		{
+			ctLaserScan.start();
+		}
 		ctUserData.join();
 		ctLaserScan.join();
 
@@ -3662,7 +3690,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	cv::Mat ground, obstacles;
 	float cellSize = 0.0f;
 	cv::Point3f viewPoint(0,0,0);
-	if(_createOccupancyGrid && !data.depthOrRightRaw().empty())
+	if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
 	{
 		_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
 		cellSize = _occupancy->getCellSize();
@@ -3817,19 +3845,12 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		{
 			const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
 			UASSERT(wordFirst!=0);
-			//Descriptors used for Memory::computeTransform()
+
-			cv::Mat descriptors(keys.size(), wordFirst->getDescriptor().cols, wordFirst->getDescriptor().type());
 			// Add all references
 			for(unsigned int i=0; i<keys.size(); ++i)
 			{
 				_vwd->addWordRef(keys.at(i), (*j)->id());
-				const VisualWord * word = _vwd->getWord(keys.at(i));
-				UASSERT(word != 0);
-
-				word->getDescriptor().copyTo(descriptors.row(i));
-
 			}
-			(*j)->sensorData().setFeatures(std::vector<cv::KeyPoint>(), descriptors);
 			(*j)->setEnabled(true);
 		}
 	}

corelib/src/Odometry.cpp

@@ -332,7 +332,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			kpts[i].size *= _imageDecimation;
 			kpts[i].octave += log2value;
 		}
-		data.setFeatures(kpts, decimatedData.descriptors());
+		data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
 
 		if(info)
 		{
@@ -496,8 +496,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 
+		if(data.stamp() == 0)
+		{
+			UWARN("Null stamp detected");
+		}
+
 		previousStamp_ = data.stamp();
 		previousVelocityTransform_.setNull();
+
 		if(dt)
 		{
 			previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
@@ -509,6 +515,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}
 
+		info->varianceLin *= t.getNorm();
+		info->varianceAng *= t.getAngle();
+
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/OdometryF2F.cpp

@@ -39,12 +39,15 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
+	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
 }
 
@@ -100,6 +103,30 @@ Transform OdometryF2F::computeTransform(
 				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
 				&regInfo);
 
+		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
+		{
+			tmpRefFrame = refFrame_;
+			// reset matches, but keep already extracted features in newFrame.sensorData()
+			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
+			newFrame.setWords3(std::multimap<int, cv::Point3f>());
+			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
+			output = registrationPipeline_->computeTransformationMod(
+				tmpRefFrame,
+				newFrame,
+				Transform(), // null guess
+				&regInfo);
+
+			if(output.isNull())
+			{
+				UWARN("Trial with no guess still fail.");
+			}
+			else
+			{
+				UWARN("Trial with no guess succeeded.");
+			}
+		}
+
 		if(info && this->isInfoDataFilled())
 		{
 			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -141,7 +168,8 @@ Transform OdometryF2F::computeTransform(
 		//return Identity
 		output = Transform::getIdentity();
 		// a very high variance tells that the new pose is not linked with the previous one
-		regInfo.variance = 9999;
+		regInfo.varianceLin = 9999;
+		regInfo.varianceAng = 9999;
 	}
 
 	if(!output.isNull())
@@ -149,8 +177,12 @@ Transform OdometryF2F::computeTransform(
 		output = motionSinceLastKeyFrame.inverse() * output;
 
 		// new key-frame?
-		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
+		if( (registrationPipeline_->isImageRequired() &&
-			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
+				(keyFrameThr_ == 0.0f ||
+				 visKeyFrameThr_ == 0 ||
+				 float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()) ||
+				 regInfo.inliers <= visKeyFrameThr_)) ||
+			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
@@ -208,12 +240,13 @@ Transform OdometryF2F::computeTransform(
 		UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
 	}
 
-	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
+	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().keypoints3D(), newFrame.sensorData().descriptors());
 
 	if(info)
 	{
 		info->type = 1;
-		info->variance = regInfo.variance;
+		info->varianceLin = regInfo.varianceLin;
+		info->varianceAng = regInfo.varianceAng;
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;

corelib/src/OdometryF2M.cpp

@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Graph.h"
+#include "rtflann/flann.hpp"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UMath.h"
@@ -58,30 +60,52 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Odometry(parameters),
 	maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
-	bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
+	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
-	regPipeline_(Registration::create(parameters)),
 	map_(new Signature(-1)),
-	lastFrame_(new Signature(1))
+	lastFrame_(new Signature(1)),
+	sba_(0)
 {
 	UDEBUG("");
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
-	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleAdjustmentMaxFrames_);
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
-	bundleParameters_ = parameters;
+	UASSERT(bundleMaxFrames_ >= 0);
+	ParametersMap bundleParameters = parameters;
+	if(bundleAdjustment_ > 0)
+	{
+		if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
+		   (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)))
+		{
+			// disable bundle in RegistrationVis as we do it already here
+			uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
+			sba_ = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
+		}
+		else
+		{
+			UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
+					"local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
+			bundleAdjustment_ = 0;
+		}
+	}
 	UASSERT(maximumMapSize_ >= 0);
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
+	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
+
+	regPipeline_ = Registration::create(bundleParameters);
 }
 
 OdometryF2M::~OdometryF2M()
@@ -94,6 +118,11 @@ OdometryF2M::~OdometryF2M()
 	bundleLinks_.clear();
 	bundleModels_.clear();
 	bundlePoseReferences_.clear();
+	if(sba_)
+	{
+		delete sba_;
+	}
+	delete regPipeline_;
 	UDEBUG("");
 }
 
@@ -103,6 +132,12 @@ void OdometryF2M::reset(const Transform & initialPose)
 	Odometry::reset(initialPose);
 	*lastFrame_ = Signature(1);
 	*map_ = Signature(-1);
+	scansBuffer_.clear();
+	bundleWordReferences_.clear();
+	bundlePoses_.clear();
+	bundleLinks_.clear();
+	bundleModels_.clear();
+	bundlePoseReferences_.clear();
 }
 
 // return not null transform if odometry is correctly computed
@@ -131,6 +166,10 @@ Transform OdometryF2M::computeTransform(
 		UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
 		bundleAdjustment_ = 0;
 	}
+	bool addKeyFrame = false;
+	int totalBundleWordReferencesUsed = 0;
+	int totalBundleOutliers = 0;
+	float bundleTime = 0.0f;
 
 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
@@ -145,20 +184,41 @@ Transform OdometryF2M::computeTransform(
 					// special case for ICP-only odom, set guess to identity if we just started
 					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
 					&regInfo);
-
+			if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
-			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
+			{
+				tmpMap = *map_;
+				// reset matches, but keep already extracted features in lastFrame_->sensorData()
+				lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
+				lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
+				lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
+				UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
+				transform = regPipeline_->computeTransformationMod(
+						tmpMap,
+						*lastFrame_,
+						Transform(), // null guess
+						&regInfo);
+				if(transform.isNull())
+				{
+					UWARN("Trial with no guess still fail.");
+				}
+				else
+				{
+					UWARN("Trial with no guess succeeded.");
+				}
+			}
+			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
 
 			std::map<int, cv::Point3f> points3DMap;
 			std::map<int, Transform> bundlePoses;
 			std::multimap<int, Link> bundleLinks;
 			std::map<int, CameraModel> bundleModels;
+			std::map<int, StereoCameraModel> bundleStereoModels;
 			if(!transform.isNull())
 			{
 				// local bundle adjustment
-				if(bundleAdjustment_>0 &&
+				if(bundleAdjustment_>0 && sba_ &&
 				   regPipeline_->isImageRequired() &&
-				   ((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
+				   lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
-			        (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA))) &&
 				   regInfo.inliersIDs.size())
 				{
 					UDEBUG("Local Bundle Adjustment");
@@ -174,51 +234,67 @@ Transform OdometryF2M::computeTransform(
 					}
 					else
 					{
-						UTimer bundleTime;
-						Optimizer * sba = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters_);
-
-						std::map<int, std::map<int, cv::Point2f> > wordReferences;
 						UASSERT(bundlePoses_.size());
-						UASSERT(bundlePoses_.size()-1 == bundleLinks_.size() && bundlePoses_.size() == bundleModels_.size());
+						UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
-						if(bundleAdjustmentMaxFrames_ > 0)
+						UASSERT(bundlePoses_.size() == bundleModels_.size());
-						{
+
-							std::map<int, Transform>::reverse_iterator iter = bundlePoses_.rbegin();
+						bundlePoses = bundlePoses_;
-							for(int i = 0; i<bundleAdjustmentMaxFrames_ && i < (int)bundlePoses_.size()-1; ++i, ++iter)
+						bundleLinks = bundleLinks_;
-							{
+						bundleModels = bundleModels_;
-								bundlePoses.insert(*iter);
+
-								UASSERT(bundleLinks_.find(iter->first) != bundleLinks_.end());
+						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin)));
-								bundleLinks.insert(*bundleLinks_.find(iter->first));
-								UASSERT(bundleModels_.find(iter->first) != bundleModels_.end());
-								bundleModels.insert(*bundleModels_.find(iter->first));
-							}
-							//make sure the origin is there
-							bundlePoses.insert(*bundlePoses_.find(0));
-							bundleModels.insert(*bundleModels_.find(0));
-						}
-						else
-						{
-							bundlePoses = bundlePoses_;
-							bundleLinks = bundleLinks_;
-							bundleModels = bundleModels_;
-						}
-						bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, transform, regInfo.variance, regInfo.variance)));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
+
+						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)
 						{
-							std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(regInfo.inliersIDs[i]);
+							int wordId =regInfo.inliersIDs[i];
+
+							// 3D point
+							std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
 							UASSERT(iter3D!=tmpMap.getWords3().end());
 							points3DMap.insert(*iter3D);
 
-							std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(regInfo.inliersIDs[i]);
+							std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
-							UASSERT(iter2D!=lastFrame_->getWords().end());
 
-							if(wordReferences.find(iter2D->first) == wordReferences.end())
+							// all other references
+							std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
+							UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
+
+							std::map<int, cv::Point3f> references;
+							int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
+							if(step == 0)
 							{
-								UASSERT(bundleWordReferences_.find(iter2D->first) != bundleWordReferences_.end());
+								step = 1;
-								wordReferences.insert(*bundleWordReferences_.find(iter2D->first));
+							}
+							int oi=0;
+							for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
+							{
+								if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
+								{
+									references.insert(*jter);
+									++totalBundleWordReferencesUsed;
+								}
+							}
+							//make sure the last reference is here
+							if(refIter->second.size() > 1)
+							{
+								references.insert(*refIter->second.rbegin());
 							}
 
-							wordReferences.find(iter2D->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
+							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)));
+							}
+							wordReferences.insert(std::make_pair(wordId, references));
+
+							//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
+							//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
+							//{
+							//	UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
+							//}
 						}
 
 						CameraModel model;
@@ -229,21 +305,61 @@ Transform OdometryF2M::computeTransform(
 						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!");
 						}
-						UASSERT(model.isValidForProjection());
 						bundleModels.insert(std::make_pair(lastFrame_->id(), model));
 
-						bundlePoses = sba->optimizeBA(0, bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences);
+						UDEBUG("sba...start");
-						delete sba;
+						// set root negative to fix all other poses
+						std::set<int> sbaOutliers;
+						UTimer bundleTimer;
+						bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
+						bundleTime = bundleTimer.ticks();
+						UDEBUG("sba...end");
+						totalBundleOutliers = (int)sbaOutliers.size();
 
-						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)bundleWordReferences_.size());
+						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
 
 						UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
-						UDEBUG("Local Bundle Adjustment After : %s", bundlePoses.rbegin()->second.prettyPrint().c_str());
+						if(bundlePoses.size() == bundlePoses_.size()+1)
-
-						if(!bundlePoses.rbegin()->second.isNull())
 						{
-							transform = bundlePoses.rbegin()->second;
+							if(!bundlePoses.rbegin()->second.isNull())
+							{
+								transform = bundlePoses.rbegin()->second;
+								bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
+
+								if(sbaOutliers.size())
+								{
+									std::vector<int> newInliers(regInfo.inliersIDs.size());
+									int oi=0;
+									for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
+									{
+										if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
+										{
+											newInliers[oi++] = regInfo.inliersIDs[i];
+										}
+									}
+									newInliers.resize(oi);
+									UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
+									regInfo.inliers = (int)newInliers.size();
+									regInfo.inliersIDs = newInliers;
+								}
+							}
+							UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
+						}
+						else
+						{
+							UWARN("Local bundle adjustment failed! transform is not refined.");
 						}
 					}
 				}
@@ -273,14 +389,25 @@ Transform OdometryF2M::computeTransform(
 				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
 				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
 
-				//Visual
+				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
-				int added = 0;
+						 (keyFrameThr_ == 0.0f ||
-				int removed = 0;
+						  visKeyFrameThr_ == 0 ||
-				UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
+						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())) ||
-				if(regPipeline_->isImageRequired() &&
+						  regInfo.inliers <= visKeyFrameThr_);
-					(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
+				bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
+
+				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
+				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
+
+				UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
+				if(addKeyFrame)
 				{
-					UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
+					//Visual
+					int added = 0;
+					int removed = 0;
+					UTimer tmpTimer;
+
+					UDEBUG("Update local map");
 
 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
@@ -290,19 +417,21 @@ Transform OdometryF2M::computeTransform(
 					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
 					if(bundleAdjustment_>0)
 					{
+						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
+						UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
+						bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
+						uInsert(bundlePoses_, bundlePoses);
+						UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
+						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
+						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
+
 						// update local map 3D points (if bundle adjustment was done)
 						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 						{
 							UASSERT(mapPoints.count(iter->first) == 1);
+							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
 							mapPoints.find(iter->first)->second = iter->second;
 						}
-						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
-						uInsert(bundlePoses_, bundlePoses);
-						UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
-						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
-						UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
-						bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
-						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
 					}
 
 					// sort by feature response
@@ -310,6 +439,8 @@ Transform OdometryF2M::computeTransform(
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
 					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
+					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
+					std::set<int> seenStatusUpdated;
 					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
 						if(util3d::isFinite(iter->second))
@@ -326,26 +457,34 @@ Transform OdometryF2M::computeTransform(
 							{
 								if(lastFrame_->getWords().count(iter->first) == 1)
 								{
+									std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
+									if(iterKpts!=mapWords.end())
+									{
+										iterKpts->second.octave = iter2D->second.octave;
+									}
+
 									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
 									iterBundlePosesRef->second += 1;
 
 									if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 									{
-										std::map<int, cv::Point2f> framePt;
+										std::map<int, cv::Point3f> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
+
+										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(), iter2D->second.pt));
+										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
 									}
 								}
 							}
 						}
 					}
+					UDEBUG("newIds=%d", (int)newIds.size());
 
-					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
+					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
-						iter!=newIds.end();
+						iter!=newIds.rend();
 						++iter)
 					{
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
@@ -359,13 +498,13 @@ Transform OdometryF2M::computeTransform(
 
 									if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
 									{
-										std::map<int, cv::Point2f> framePt;
+										std::map<int, cv::Point3f> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
+										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)));
 										bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
+										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)));
 									}
 								}
 							}
@@ -380,7 +519,7 @@ Transform OdometryF2M::computeTransform(
 					// remove words in map if max size is reached
 					if((int)mapPoints.size() > maximumMapSize_)
 					{
-						// remove oldest first, keep matched features
+						// remove oldest first, keep matched features with their aliases
 						std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
 						std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
 						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
@@ -389,22 +528,14 @@ Transform OdometryF2M::computeTransform(
 						{
 							if(matches.find(iter->first) == matches.end())
 							{
-								std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
+								std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
 								if(iterRef != bundleWordReferences_.end())
 								{
-									for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
+									for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
 									{
-										if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
+										if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
 										{
-											bundlePoseReferences_.at(iterKp->first) -= 1;
+											bundlePoseReferences_.at(iterFrame->first) -= 1;
-											if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
-											{
-												bundlePoses_.erase(iterKp->first);
-												bundleLinks_.erase(iterKp->first);
-												bundleModels_.erase(iterKp->first);
-												bundlePoseReferences_.erase(iterKp->first);
-												UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
-											}
 										}
 									}
 									bundleWordReferences_.erase(iterRef);
@@ -422,18 +553,34 @@ Transform OdometryF2M::computeTransform(
 								++iterMapWords;
 							}
 						}
-					}
-					modified = true;
-				}
 
-				// Geometric
+						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
-				UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
+						{
-				if(regPipeline_->isScanRequired() &&
+							if((iter->second <= 0 && // <= regPipeline_->getMinVisualCorrespondences() &&
-					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
+								bundlePoses_.begin()->first == iter->first)) // remove oldest pose first
-				{
+							{
+								UASSERT(bundlePoses_.erase(iter->first) == 1);
+								bundleLinks_.erase(iter->first);
+								bundleModels_.erase(iter->first);
+								bundlePoseReferences_.erase(iter++);
+							}
+							else
+							{
+								++iter;
+							}
+						}
+					}
+
+					if(added || removed)
+					{
+						modified = true;
+					}
+					UDEBUG("Update local features map = %fs", tmpTimer.ticks());
+
+					// Geometric
+					UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
 					UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
 
-					UTimer tmpTimer;
 					if(lastFrame_->sensorData().laserScanRaw().cols)
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
@@ -540,7 +687,7 @@ Transform OdometryF2M::computeTransform(
 							modified=true;
 						}
 					}
-					UDEBUG("Update local map = %fs", tmpTimer.ticks());
+					UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
 				}
 
 				if(modified)
@@ -550,7 +697,7 @@ Transform OdometryF2M::computeTransform(
 					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
 					map_->setWords(mapWords);
 					map_->setWords3(mapPoints);
-					map_->setWordsDescriptors(mapDescriptors);
+				 	map_->setWordsDescriptors(mapDescriptors);
 				}
 			}
 
@@ -577,10 +724,11 @@ Transform OdometryF2M::computeTransform(
 						dummy);
 			}
 
-			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
+			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
 
 			// a very high variance tells that the new pose is not linked with the previous one
-			regInfo.variance = 9999;
+			regInfo.varianceLin = 9999;
+			regInfo.varianceAng = 9999;
 
 			bool frameValid = false;
 			Transform newFramePose = this->getPose(); // initial pose may be not identity...
@@ -619,11 +767,20 @@ Transform OdometryF2M::computeTransform(
 							if(words.count(iter->first) == 1)
 							{
 								UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
-								std::map<int, cv::Point2f> framePt;
+								std::map<int, cv::Point3f> framePt;
-								framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
+
+								//get depth
+								float d = 0.0f;
+								if(lastFrame_->getWords3().count(iter->first) == 1)
+								{
+									d = lastFrame_->getWords3().find(iter->first)->second.x;
+								}
+
+								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));
 							}
 						}
+
 						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
 
 						CameraModel model;
@@ -634,24 +791,28 @@ Transform OdometryF2M::computeTransform(
 						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("invalid camera model!");
 						}
-						UASSERT(model.isValidForProjection());
-						UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
 						bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
-						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
-						bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
 
-						//origin
+						UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
-						bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
+						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
-						bundleModels_.insert(std::make_pair(0, model));
 					}
 
 					map_->setWords(words);
 					map_->setWords3(transformedPoints);
 					map_->setWordsDescriptors(descriptors);
-
+					addKeyFrame = true;
-					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
-					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
 				}
 				else
 				{
@@ -666,6 +827,7 @@ Transform OdometryF2M::computeTransform(
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
 					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
+					addKeyFrame = true;
 				}
 				else
 				{
@@ -692,7 +854,7 @@ Transform OdometryF2M::computeTransform(
 			}
 		}
 
-		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
+		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features
 
 		nFeatures = lastFrame_->getWords().size();
 		if(this->isInfoDataFilled() && info)
@@ -706,11 +868,17 @@ Transform OdometryF2M::computeTransform(
 
 	if(info)
 	{
-		info->variance = regInfo.variance;
+		info->varianceLin = regInfo.varianceLin;
+		info->varianceAng = regInfo.varianceAng;
 		info->inliers = regInfo.inliers;
 		info->matches = regInfo.matches;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
+		info->localKeyFrames = (int)bundlePoses_.size();
+		info->keyFrameAdded = addKeyFrame;
+		info->localBundleOutliers = totalBundleOutliers;
+		info->localBundleConstraints = totalBundleWordReferencesUsed;
+		info->localBundleTime = bundleTime;
 
 		if(this->isInfoDataFilled())
 		{
@@ -719,15 +887,17 @@ Transform OdometryF2M::computeTransform(
 		}
 	}
 
-	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d local_scan_map=%d",
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			nFeatures,
 			regInfo.inliers,
 			regInfo.matches,
-			regInfo.variance,
+			regInfo.varianceLin,
+			regInfo.varianceAng,
 			regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
 			regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
+
 	return output;
 }
 

corelib/src/OdometryMono.cpp

@@ -976,7 +976,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->variance = 9999;
+			info->varianceLin = 9999;
+			info->varianceAng = 9999;
 		}
 
 			// generate kpts

corelib/src/OdometryThread.cpp

@@ -101,9 +101,10 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
-		double variance = info.variance>0?info.variance:1;
+		double varianceLin = info.varianceLin>0?info.varianceLin:1;
+		double varianceAng = info.varianceAng>0?info.varianceAng:1;
 		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-		this->post(new OdometryEvent(data, pose, variance, variance, info));
+		this->post(new OdometryEvent(data, pose, varianceAng, varianceLin, info));
 	}
 }
 

corelib/src/Optimizer.cpp

@@ -267,7 +267,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		const std::multimap<int, Link> & links,
 		const std::map<int, CameraModel> & models,
 		std::map<int, cv::Point3f> & points3DMap,
-		const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
 	return std::map<int, Transform>();
@@ -294,6 +295,15 @@ std::map<int, Transform> Optimizer::optimizeBA(
 			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
 			{
 				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+
+				// Set Tx = -baseline*fx for stereo BA
+				model = CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						model.localTransform(),
+						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
 			}
 			else
 			{
@@ -314,7 +324,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 
 	// compute correspondences
 	std::map<int, cv::Point3f> points3DMap;
-	std::map<int, std::map<int, cv::Point2f> > wordReferences;
+	std::map<int, std::map<int, cv::Point3f> > wordReferences;
 	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
 
 	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
@@ -324,7 +334,8 @@ Transform Optimizer::optimizeBA(
 		const Link & link,
 		const CameraModel & model,
 		std::map<int, cv::Point3f> & points3DMap,
-		const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	std::map<int, Transform> poses;
 	poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
@@ -334,7 +345,7 @@ Transform Optimizer::optimizeBA(
 	std::map<int, CameraModel> models;
 	models.insert(std::make_pair(link.from(), model));
 	models.insert(std::make_pair(link.to(), model));
-	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences);
+	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
 	if(poses.size() == 2)
 	{
 		return poses.rbegin()->second;
@@ -350,7 +361,7 @@ void Optimizer::computeBACorrespondences(
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
-		std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
+		std::map<int, std::map<int, cv::Point3f> > & wordReferences) // <ID words, IDs frames + keypoint/depth>
 {
 	UDEBUG("");
 	int wordCount = 0;
@@ -367,49 +378,75 @@ void Optimizer::computeBACorrespondences(
 		   uContains(poses, link.from()))
 		{
 			Signature sFrom = signatures.at(link.from());
-			Signature sTo = signatures.at(link.to());
+			if(sFrom.getWeight() >= 0) // ignore intermediate links
-
-			if(sFrom.getWords().size() &&
-				sTo.getWords().size() &&
-				sFrom.getWords3().size())
 			{
-				ParametersMap regParam;
+				Signature sTo = signatures.at(link.to());
-				regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+				if(sTo.getWeight() < 0)
-				regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-				RegistrationVis reg(regParam);
-
-				//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-
-				RegistrationInfo info;
-				Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
-				//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
-				UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
-
-				if(!t.isNull())
 				{
-					Transform pose = poses.at(sFrom.id());
+					for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
-					for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+						sTo.getWeight() < 0 && jter!=links.end() &&  uContains(signatures, jter->second.to());
+						++jter)
 					{
-						cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+						sTo = signatures.at(jter->second.to());
-						if(p.x > 0.0f) // make sure the point is valid
-						{
-							int wordId = ++wordCount;
-
-							p = util3d::transformPoint(p, pose);
-							points3DMap.insert(std::make_pair(wordId, p));
-							wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
-							wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-							wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-						}
 					}
-					++edgeWithWordsAdded;
 				}
-				else
+
+				if(sFrom.getWords().size() &&
+					sTo.getWords().size() &&
+					sFrom.getWords3().size())
 				{
-					UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					ParametersMap regParam;
+					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+					RegistrationVis reg(regParam);
+
+					//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+					//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+
+					RegistrationInfo info;
+					Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
+					//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+					UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
+
+					if(!t.isNull())
+					{
+						Transform pose = poses.at(sFrom.id());
+						UASSERT(!pose.isNull());
+						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+						{
+							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+							if(p.x > 0.0f) // make sure the point is valid
+							{
+								int wordId = ++wordCount;
+
+								wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
+
+								cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
+
+
+								pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								float depth = 0.0f;
+								std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
+								if( iterTo!=sTo.getWords3().end() &&
+									iterTo->second.x > 0)
+								{
+									depth = iterTo->second.x;
+								}
+								wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
+
+								p = util3d::transformPoint(p, pose);
+								points3DMap.insert(std::make_pair(wordId, p));
+							}
+						}
+						++edgeWithWordsAdded;
+					}
+					else
+					{
+						UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					}
 				}
 			}
 		}

corelib/src/OptimizerCVSBA.cpp

@@ -59,7 +59,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		const std::multimap<int, Link> & links,
 		const std::map<int, CameraModel> & models,
 		std::map<int, cv::Point3f> & points3DMap,
-		const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
+		std::set<int> * outliers)
 {
 #ifdef RTABMAP_CVSBA
 	// run sba optimization
@@ -130,14 +131,14 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	{
 		points[i] = kter->second;
 
-		std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.find(kter->first);
+		std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.find(kter->first);
 		if(iter != wordReferences.end())
 		{
-			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 			{
 				if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
 				{
-					imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
+					imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.x, jter->second.y);
 					visibility[frameIdToIndex.at(jter->first)][i] = 1;
 				}
 			}

corelib/src/OptimizerG2O.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OptimizerG2O.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
+#include <rtabmap/core/util3d.h>
 
 #ifdef RTABMAP_G2O
 #include "g2o/config.h"
@@ -54,12 +55,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
+#include "g2o/solvers/eigen/linear_solver_eigen.h"
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/edge_se3.h"
 #include "g2o/types/slam2d/vertex_se2.h"
 #include "g2o/types/slam2d/edge_se2.h"
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
+typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
 #ifdef G2O_HAVE_CSPARSE
 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -113,7 +116,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
 	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
 	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(baseline_ >= 0.0);
 
 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
@@ -162,7 +168,14 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 		SlamBlockSolver * blockSolver = 0;
 
-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = new SlamBlockSolver(linearSolver);
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -545,21 +558,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		const std::multimap<int, Link> & links,
 		const std::map<int, CameraModel> & models,
 		std::map<int, cv::Point3f> & points3DMap,
-		const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");
 
 	optimizedPoses.clear();
-	if(links.size()>=1 && poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
+	if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
 	{
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
-		bool robustKernel = true;
 
-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -599,8 +617,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
 
 			Transform camPose = iter->second * iterModel->second.localTransform();
-			//iter->second = (iter->second * model.localTransform()).inverse();
-			UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
 
 			// Add node's pose
 			UASSERT(!camPose.isNull());
@@ -608,20 +624,35 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 			Eigen::Affine3d a = camPose.toEigen3d();
 			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
-			cam.setKcam(iterModel->second.fx(), iterModel->second.fy(), iterModel->second.cx(), iterModel->second.cy(), 0);
+			cam.setKcam(
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
 			vCam->setEstimate(cam);
-			if(iter->first == rootId)
-			{
-				vCam->setFixed(true);
-			}
 			vCam->setId(iter->first);
-			//std::cout << cam << std::endl;
+
+			// negative root means that all other poses should be fixed instead of the root
+			vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
+
+			UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
+					iter->first,
+					vCam->fixed()?1:0,
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
+					camPose.prettyPrint().c_str());
+
 			UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
 
 			++iter;
 		}
 
-		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
+		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
 			if(uContains(poses, iter->second.from()) &&
@@ -641,17 +672,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 				// between cameras, not base_link
 				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-				//Transform t = iter->second.transform();
+				UDEBUG("added edge %d->%d (in cam frame=%s)",
-				UDEBUG("added edge %d=%s -> %d=%s",
 						id1,
-						iter->second.transform().prettyPrint().c_str(),
 						id2,
 						camLink.prettyPrint().c_str());
 				Eigen::Affine3d a = camLink.toEigen3d();
 
 				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
-				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
+				g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
-				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
 				UASSERT(v1 != 0);
 				UASSERT(v2 != 0);
 				e->setVertex(0, v1);
@@ -669,8 +698,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		}
 
 		UDEBUG("fill 3D points to g2o...");
-		int stepVertexId = poses.rbegin()->first+1;
+		const int stepVertexId = poses.rbegin()->first+1;
-		for(std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+		std::list<g2o::OptimizableGraph::Edge*> edges;
+		for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
 			if(points3DMap.find(iter->first) != points3DMap.end())
 			{
@@ -682,32 +712,61 @@ 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);
+
 				// set observations
-				for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 				{
 					int camId = jter->first;
-					if(poses.find(camId) != poses.end())
+					if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
 					{
-						const cv::Point2f & pt = jter->second;
+						const cv::Point3f & pt = jter->second;
+						double depth = pt.z;
 
-						Eigen::Matrix<double,2,1> obs;
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
-						obs << pt.x, pt.y;
 
-						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
+						g2o::OptimizableGraph::Edge * e;
-
+						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
-						g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
+						double variance = pixelVariance_;
-
+						if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0)
-						e->setVertex(0, vpt3d);
-						e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
-						e->setMeasurement(obs);
-						e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
-
-						if(robustKernel)
 						{
-							e->setRobustKernel(new g2o::RobustKernelHuber);
+							// stereo edge
+							g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
+							float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth;
+							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
+							es->setMeasurement(obs);
+							//variance *= log(exp(1)+disparity);
+							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							e = es;
+						}
+						else
+						{
+							if(vcam->estimate().baseline > 0.0)
+							{
+								UWARN("Stereo camera model detected but current "
+										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
+										"mono observation instead.",
+										vpt3d->id()-stepVertexId, camId, depth);
+							}
+							// mono edge
+							g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
+							Eigen::Vector2d obs( pt.x, pt.y);
+							em->setMeasurement(obs);
+							em->setInformation(Eigen::Matrix2d::Identity() / variance);
+							e = em;
+						}
+						e->setVertex(0, vpt3d);
+						e->setVertex(1, vcam);
+
+						if(robustKernelDelta_ > 0.0)
+						{
+							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
+							kernel->setDelta(robustKernelDelta_);
+							e->setRobustKernel(kernel);
 						}
 
 						optimizer.addEdge(e);
+						edges.push_back(e);
 					}
 				}
 			}
@@ -718,56 +777,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		UASSERT(optimizer.verifyInformationMatrices());
 
-		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
+		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%f)", iterations(), this->epsilon(), robustKernelDelta_);
 
 		int it = 0;
 		UTimer timer;
-		double lastError = 0.0;
+		int outliersCount = 0;
-		if(this->epsilon() > 0.0)
+		int outliersCountFar = 0;
+
+		for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
 		{
-			for(int i=0; i<iterations(); ++i)
+			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?3:iterations());
+
+			// early stop condition
+			optimizer.computeActiveErrors();
+			double chi2 = optimizer.activeRobustChi2();
+			if(uIsNan(chi2))
 			{
-				it += optimizer.optimize(1);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				return optimizedPoses;
+			}
+			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
 
-				// early stop condition
+			if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
-				optimizer.computeActiveErrors();
+			{
-				double chi2 = optimizer.activeRobustChi2();
+				UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
-				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+				return optimizedPoses;
+			}
 
-				if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+			if(robustKernelDelta_>0.0)
+			{
+				for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
 				{
-					UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
+					if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
-					return optimizedPoses;
-				}
-
-				double errorDelta = lastError - chi2;
-				if(i>0 && errorDelta < this->epsilon())
-				{
-					if(errorDelta < 0)
 					{
-						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+						(*iter)->setLevel(1);
-					}
+						++outliersCount;
-					else
+						double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
-					{
+						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());
-						UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+
-						break;
+						const 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)
+						{
+							outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
+						}
+						if(d < 5.0)
+						{
+							outliersCountFar++;
+						}
 					}
+					//(*iter)->setRobustKernel(0);
 				}
-				else if(i==0 && chi2 < this->epsilon())
+				if(i==0)
-				{
+					optimizer.initializeOptimization(0);
-					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
+				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
-					break;
-				}
-				lastError = chi2;
 			}
 		}
-		else
+
-		{
+		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());
-			it = optimizer.optimize(iterations());
-			optimizer.computeActiveErrors();
-			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
-		}
-		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
 
 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{
@@ -782,10 +850,16 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());
+
 				// 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());
+				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
-				UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+				if(t.isNull())
+				{
+					UERROR("Optimized pose %d is null!?!?", iter->first);
+					optimizedPoses.clear();
+					return optimizedPoses;
+				}
 
 				// FIXME: is there a way that we can add the 2D constraint directly in SBA?
 				if(this->isSlam2d())
@@ -806,6 +880,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		}
 
 		//update points3D
+
 		for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 		{
 			const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
@@ -817,7 +892,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			}
 			else
 			{
-				UERROR("Vertex (point3D) %d not found!?", iter->first);
+				iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
 			}
 		}
 	}

corelib/src/Registration.cpp

@@ -197,13 +197,16 @@ Transform Registration::computeTransformationMod(
 	{
 		if(info.icpInliersRatio)
 		{
-			info.variance = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
+			info.varianceLin = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
+			info.varianceAng = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
 		}
 		else
 		{
-			info.variance = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+			info.varianceLin = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+			info.varianceAng = info.inliers > 0?1.0f/float(info.inliers):1.0f;
 		}
-		info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform
+		info.varianceLin = info.varianceLin>0.0f?info.varianceLin:0.0001f; // epsilon if exact transform
+		info.varianceAng = info.varianceAng>0.0f?info.varianceAng:0.0001f; // epsilon if exact transform
 	}
 
 	if(child_)

corelib/src/RegistrationIcp.cpp

@@ -360,7 +360,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);
 
-					info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					info.varianceLin = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					info.varianceAng = variance>0.0f?variance:0.0001; // epsilon if exact transform
 					info.icpInliersRatio = correspondencesRatio;
 
 					if(correspondencesRatio < _correspondenceRatio)

corelib/src/RegistrationVis.cpp

@@ -93,6 +93,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
+	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
 	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
@@ -193,20 +194,22 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());
 
-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d descriptors",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
 			(int)fromSignature.getWordsDescriptors().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
+			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows);
 
-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
 			(int)toSignature.getWordsDescriptors().size(),
 			(int)toSignature.sensorData().keypoints().size(),
+			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows);
 
 	std::string msg;
@@ -327,13 +330,17 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 
 			std::vector<cv::Point3f> kptsFrom3D;
-			if(fromSignature.getWords3().empty())
+			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
+				kptsFrom3D = uValues(fromSignature.getWords3());
+			}
+			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			{
+				kptsFrom3D = fromSignature.sensorData().keypoints3D();
 			}
 			else
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
 			}
 
 			if(!imageTo.empty())
@@ -402,6 +409,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom.resize(ki);
 				kptsTo.resize(ki);
 				kptsFrom3DKept.resize(ki);
+				kptsFrom3D = kptsFrom3DKept;
 
 				std::vector<cv::Point3f> kptsTo3D;
 				if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
@@ -423,7 +431,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						words3To.insert(std::make_pair(id, kptsTo3D[i]));
 					}
 				}
-				toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
+				toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
 			}
 			else
 			{
@@ -437,10 +445,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
 					}
 				}
-				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}
 
-			fromSignature.sensorData().setFeatures(kptsFrom, cv::Mat());
+			fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, cv::Mat());
 		}
 		else // Features Matching
 		{
@@ -514,6 +522,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
 					imageFrom = tmp;
 				}
+				orignalWordsFromIds.clear();
 				descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
 			}
 
@@ -553,23 +562,45 @@ Transform RegistrationVis::computeTransformationImpl(
 			// create 3D keypoints
 			std::vector<cv::Point3f> kptsFrom3D;
 			std::vector<cv::Point3f> kptsTo3D;
-			if(fromSignature.getWords3().empty() || (kptsFrom.size() && kptsFrom.size() != fromSignature.getWords3().size()))
+			if(kptsFrom.size() == fromSignature.getWords3().size())
+			{
+				kptsFrom3D = uValues(fromSignature.getWords3());
+			}
+			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			{
+				kptsFrom3D = fromSignature.sensorData().keypoints3D();
+			}
+			else
 			{
 				if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
 				{
 					UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
-						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).");
+						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).",
+						   kptsFrom.size(),
+						   fromSignature.getWords3().size());
+				}
+				else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
+				{
+					UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
+						   "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsfrom).",
+						   kptsFrom.size(),
+						   fromSignature.sensorData().keypoints3D().size());
 				}
 				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
+				UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
 				if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
 				{
-					UDEBUG("");
 					//remove all keypoints/descriptors with no valid 3D points
 					UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
 							kptsFrom3D.size() == kptsFrom.size());
 					std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
 					std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
 					cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
+					std::vector<int> validKeypointsIds;
+					if(orignalWordsFromIds.size())
+					{
+						validKeypointsIds.resize(kptsFrom.size());
+					}
 
 					int oi=0;
 					for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
@@ -578,6 +609,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						{
 							validKeypoints[oi] = kptsFrom[i];
 							validKeypoints3D[oi] = kptsFrom3D[i];
+							if(orignalWordsFromIds.size())
+							{
+								validKeypointsIds[oi] = orignalWordsFromIds[i];
+							}
 							descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
 							++oi;
 						}
@@ -587,19 +622,39 @@ Transform RegistrationVis::computeTransformationImpl(
 					validKeypoints3D.resize(oi);
 					kptsFrom = validKeypoints;
 					kptsFrom3D = validKeypoints3D;
+					
+					if(orignalWordsFromIds.size())
+					{
+					validKeypointsIds.resize(oi);
+						orignalWordsFromIds = validKeypointsIds;
+					}
 					descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
 				}
 			}
-			else
+
+			if(kptsTo.size() == toSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsTo3D = uValues(toSignature.getWords3());
 			}
-			if(toSignature.getWords3().empty() || kptsTo.size() != toSignature.getWords3().size())
+			else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
+			{
+				kptsTo3D = toSignature.sensorData().keypoints3D();
+			}
+			else
 			{
 				if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
 				{
 					UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
-						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
+						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).",
+						   (int)kptsTo.size(),
+						   (int)toSignature.getWords3().size());
+				}
+				else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
+				{
+					UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
+						   "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo).",
+						   (int)kptsTo.size(),
+						   (int)toSignature.sensorData().keypoints3D().size());
 				}
 				kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
 				if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
@@ -631,15 +686,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					descriptorsTo = validDescriptors.rowRange(0, oi).clone();
 				}
 			}
-			else
-			{
-				kptsTo3D = uValues(toSignature.getWords3());
-			}
 
 			UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);
 
-			fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
+			fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, descriptorsFrom);
-			toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
+			toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, descriptorsTo);
 
 			UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
 			UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
@@ -651,9 +702,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool isCalibrated = false; // multiple cameras not supported.
 				if(imageSize.height == 0 || imageSize.width == 0)
 				{
-					imageSize = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+					imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
 				}
-				isCalibrated = imageSize.height != 0 && imageSize.width != 0 && fromSignature.sensorData().cameraModels().size()==1?fromSignature.sensorData().cameraModels()[0].isValidForProjection():fromSignature.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();
@@ -665,12 +716,12 @@ Transform RegistrationVis::computeTransformationImpl(
 					UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
 
 					// Use guess to project 3D "from" keypoints into "to" image
-					if(fromSignature.sensorData().cameraModels().size() > 1)
+					if(toSignature.sensorData().cameraModels().size() > 1)
 					{
 						UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
 					}
 
-					Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
+					Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
 					Transform guessCameraRef = (guess * localTransform).inverse();
 					cv::Mat R = (cv::Mat_<double>(3,3) <<
 							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
@@ -679,7 +730,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					cv::Mat rvec(1,3, CV_64FC1);
 					cv::Rodrigues(R, rvec);
 					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-					cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
+					cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
 					std::vector<cv::Point2f> projected;
 					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
 
@@ -688,10 +739,12 @@ 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)))
+						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
+						   util3d::transformPoint(kptsFrom3D[i], guessInv).x > 0.0)
 						{
 							projectedIndexToDescIndex[oi] = i;
 							cornersProjected[oi++] = projected[i];
@@ -712,7 +765,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 						// Create kd-tree for projected keypoints
 						rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-						rtflann::Index<rtflann::L2<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
+						rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
 						index.buildIndex();
 
 						std::vector< std::vector<size_t> > indices;
@@ -728,10 +781,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
 						UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
 						UASSERT(pointsToMat.rows == kptsTo.size());
-						UDEBUG("");
+						UDEBUG("radius search done for guess");
 
 						// Process results (Nearest Neighbor Distance Ratio)
-						int newToId = descriptorsFrom.rows;
+						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;
@@ -748,8 +801,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									int oi=0;
 									for(unsigned int j=0; j<indices[i].size(); ++j)
 									{
-										if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave>=octave-1 &&
+										if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
-										   kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave<=octave+1)
 										{
 											descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
 											descriptorsIndices[oi++] = indices[i].at(j);
@@ -774,8 +826,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									}
 								}
 								else if(indices[i].size() == 1 &&
-									    kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave >= octave-1 &&
+									    kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
-										kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave <= octave+1)
 								{
 									matchedIndex = indices[i].at(0);
 								}
@@ -849,7 +900,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						int oi=0;
 						for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
 						{
-							if(iter->first >= descriptorsFrom.rows+descriptorsTo.rows && wordsTo.count(iter->first) <= 1)
+							if(iter->first < (orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows) && wordsTo.count(iter->first) <= 1)
 							{
 								matches[oi++] = iter->second;
 							}
@@ -857,10 +908,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						matches.resize(oi);
 						UDEBUG("guess=%s", guess.prettyPrint().c_str());
 						std::vector<cv::KeyPoint> projectedKpts;
-						cv::KeyPoint::convert(projected, projectedKpts);
+						cv::KeyPoint::convert(cornersProjected, projectedKpts);
 						cv::Mat image = toSignature.sensorData().imageRaw().clone();
-						drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
 						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");*/
@@ -1167,38 +1218,57 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 		}
 
-		if(!transforms[1].isNull())
-		{
-			transforms[1] = transforms[1].inverse();
-		}
-
 		if(!_forwardEstimateOnly)
 		{
 			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
-			UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
+			UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
+		}
+
+		std::vector<int> allInliers = inliers[0];
+		if(inliers[1].size())
+		{
+			std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
+			unsigned int oi = allInliers.size();
+			allInliers.resize(allInliers.size() + inliers[1].size());
+			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			{
+				if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
+				{
+					allInliers[oi++] = inliers[1][i];
+				}
+			}
+			allInliers.resize(oi);
+		}
+		std::vector<int> allMatches = matches[0];
+		if(matches[1].size())
+		{
+			std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
+			unsigned int oi = allMatches.size();
+			allMatches.resize(allMatches.size() + matches[1].size());
+			for(unsigned int i=0; i<matches[1].size(); ++i)
+			{
+				if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
+				{
+					allMatches[oi++] = matches[1][i];
+				}
+			}
+			allMatches.resize(oi);
 		}
 
 		if(_bundleAdjustment > 0 &&
 			_estimationType < 2 &&
 			!transforms[0].isNull() &&
-			inliers[0].size() &&
+			allInliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
 			fromSignature.sensorData().cameraModels().size() <= 1 &&
 			toSignature.sensorData().cameraModels().size() <= 1)
 		{
-			UASSERT(fromSignature.sensorData().stereoCameraModel().isValidForProjection() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
+			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
-			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+
 			std::map<int, Transform> poses;
 			std::multimap<int, Link> links;
-			std::map<int, CameraModel> models;
 			std::map<int, cv::Point3f> points3DMap;
-			std::map<int, std::map<int, cv::Point2f> > wordReferences;
-
-			const CameraModel & cameraModelFrom = fromSignature.sensorData().stereoCameraModel().isValidForProjection()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
-			const CameraModel & cameraModelTo = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
-			models.insert(std::make_pair(1, cameraModelFrom));
-			models.insert(std::make_pair(2, cameraModelTo));
 
 			poses.insert(std::make_pair(1, Transform::getIdentity()));
 			poses.insert(std::make_pair(2, transforms[0]));
@@ -1209,57 +1279,81 @@ Transform RegistrationVis::computeTransformationImpl(
 				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], variances[1], variances[1])));
 			}
 
-			for(unsigned int i=0; i<inliers[0].size(); ++i)
+			std::map<int, Transform> optimizedPoses;
+
+			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
+					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+
+			std::map<int, CameraModel> models;
+
+			CameraModel cameraModelFrom;
+			if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				points3DMap.insert(*fromSignature.getWords3().find(inliers[0][i]));
+				cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
-				std::map<int, cv::Point2f> ptMap;
+				// Set Tx=-baseline*fx for Stereo BA
-				/*if(fromSignature.getWords().size())
+				cameraModelFrom = CameraModel(cameraModelFrom.fx(),
-				{
+						cameraModelFrom.fy(),
-					ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[0][i])->second.pt));
+						cameraModelFrom.cx(),
-				}*/
+						cameraModelFrom.cy(),
-				if(toSignature.getWords().size())
+						cameraModelFrom.localTransform(),
-				{
+						-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
-					ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[0][i])->second.pt));
+			}
-				}
+			else if(fromSignature.sensorData().cameraModels().size() == 1)
-				wordReferences.insert(std::make_pair(inliers[0][i], ptMap));
+			{
+				cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
 			}
 
-			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			CameraModel cameraModelTo;
+			if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				std::multimap<int, cv::Point3f>::const_iterator iter = fromSignature.getWords3().find(inliers[1][i]);
+				cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
-				if(iter!=fromSignature.getWords3().end())
+				// Set Tx=-baseline*fx for Stereo BA
-				{
+				cameraModelTo = CameraModel(cameraModelTo.fx(),
-					std::map<int, std::map<int, cv::Point2f> >::iterator jter = wordReferences.find(inliers[1][i]);
+						cameraModelTo.fy(),
-					if(jter == wordReferences.end())
+						cameraModelTo.cx(),
-					{
+						cameraModelTo.cy(),
-						points3DMap.insert(*fromSignature.getWords3().find(inliers[1][i]));
+						cameraModelTo.localTransform(),
-						std::map<int, cv::Point2f> ptMap;
+						-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
-						if(fromSignature.getWords().size())
+			}
-						{
+			else if(toSignature.sensorData().cameraModels().size() == 1)
-							ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
+			{
-						}
+				cameraModelTo = toSignature.sensorData().cameraModels()[0];
-						if(toSignature.getWords().size())
-						{
-							ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[1][i])->second.pt));
-						}
-						wordReferences.insert(std::make_pair(inliers[1][i], ptMap));
-					}
-					else
-					{
-						if(jter->second.find(1) == jter->second.end())
-						{
-							jter->second.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
-						}
-						if(jter->second.find(2) == jter->second.end())
-						{
-							jter->second.insert(std::make_pair(1, toSignature.getWords().find(inliers[1][i])->second.pt));
-						}
-					}
-				}
 			}
 
-			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
+			models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
-			std::map<int, Transform> optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences);
+			models.insert(std::make_pair(2, cameraModelTo));
+
+			std::map<int, std::map<int, cv::Point3f> > wordReferences;
+			for(unsigned int i=0; i<allInliers.size(); ++i)
+			{
+				int wordId = allInliers[i];
+				const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
+				points3DMap.insert(std::make_pair(wordId, pt3D));
+
+				std::map<int, cv::Point3f> ptMap;
+				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
+				{
+					const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
+					ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, pt3D.x)));
+				}
+				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
+				{
+					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)));
+				}
+
+				wordReferences.insert(std::make_pair(wordId, ptMap));
+
+				//UDEBUG("%d (%f,%f,%f)", wordId, points3DMap.at(wordId).x, points3DMap.at(wordId).y, points3DMap.at(wordId).z);
+				//for(std::map<int, cv::Point3f>::iterator iter=ptMap.begin(); iter!=ptMap.end(); ++iter)
+				//{
+				//	UDEBUG("%d (%f,%f) d=%f", iter->first, iter->second.x, iter->second.y, iter->second.z);
+				//}
+			}
+
+			std::set<int> sbaOutliers;
+			optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
 			delete sba;
 
 			//update transform
@@ -1267,6 +1361,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				!optimizedPoses.begin()->second.isNull() &&
 				!optimizedPoses.rbegin()->second.isNull())
 			{
+				UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
 				transforms[0] = optimizedPoses.rbegin()->second;
 				transforms[1].setNull();
 				// update 3D points, both from and to signatures
@@ -1283,41 +1378,47 @@ Transform RegistrationVis::computeTransformationImpl(
 				}
 				fromSignature.setWords3(cpyWordsFrom3);
 				toSignature.setWords3(cpyWordsTo3);*/
+
+				if(sbaOutliers.size())
+				{
+					std::vector<int> newInliers(allInliers.size());
+					int oi=0;
+					for(unsigned int i=0; i<allInliers.size(); ++i)
+					{
+						if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
+						{
+							newInliers[oi++] = allInliers[i];
+						}
+					}
+					newInliers.resize(oi);
+					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
+					allInliers = newInliers;
+				}
 			}
 		}
 
+		info.inliersIDs = allInliers;
+		info.matchesIDs = allMatches;
+		inliersCount = (int)allInliers.size();
+		matchesCount = (int)allMatches.size();
 		if(!transforms[1].isNull())
 		{
+			transforms[1] = transforms[1].inverse();
 			if(transforms[0].isNull())
 			{
 				transform = transforms[1];
-				info.inliersIDs = inliers[1];
-				info.matchesIDs = matches[1];
-
 				variance = variances[1];
-				inliersCount = (int)inliers[1].size();
-				matchesCount = (int)matches[1].size();
 			}
 			else
 			{
 				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				info.inliersIDs = inliers[0];
-				info.matchesIDs = matches[0];
-
 				variance = (variances[0]+variances[1])/2.0f;
-				inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
-				matchesCount = (int)(matches[0].size()+matches[1].size())/2;
 			}
 		}
 		else
 		{
 			transform = transforms[0];
-			info.inliersIDs = inliers[0];
-			info.matchesIDs = matches[0];
-
 			variance = variances[0];
-			inliersCount = (int)inliers[0].size();
-			matchesCount = (int)matches[0].size();
 		}
 	}
 	else if(toSignature.sensorData().isValid())
@@ -1329,7 +1430,8 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.inliers = inliersCount;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
-	info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
+	info.varianceLin = variance>0.0f?variance:0.0001f; // epsilon if exact transform
+	info.varianceAng = info.varianceLin;
 
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;

corelib/src/Rtabmap.cpp

@@ -981,6 +981,9 @@ bool Rtabmap::process(
 	std::list<int> signaturesRemoved;
 	if(_rgbdSlamMode)
 	{
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), covariance.empty()?1.0f:(float)covariance.at<double>(0,0));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), covariance.empty()?1.0f:(float)covariance.at<double>(5,5));
+
 		//Verify if there was a rehearsal
 		int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		if(rehearsedId > 0)
@@ -1029,84 +1032,92 @@ bool Rtabmap::process(
 			const Signature * oldS = _memory->getSignature(oldId);
 			UASSERT(oldS != 0);
 
-			Transform guess = signature->getLinks().begin()->second.transform().inverse();
+			if(signature->getWeight() >= 0 && oldS->getWeight()>=0) // ignore intermediate nodes
-
-			if(smallDisplacement)
 			{
-				if(signature->getLinks().begin()->second.transVariance() == 1)
+				Transform guess = signature->getLinks().begin()->second.transform().inverse();
+
+				if(smallDisplacement)
 				{
-					// set small variance
+					if(signature->getLinks().begin()->second.transVariance() == 1)
-					UDEBUG("Set small variance. The robot is not moving.");
+					{
-					_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
+						// set small variance
+						UDEBUG("Set small variance. The robot is not moving.");
+						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
+					}
+				}
+				else
+				{
+					//============================================================
+					// Refine neighbor links
+					//============================================================
+					if(!signature->sensorData().laserScanCompressed().empty())
+					{
+						UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
+						RegistrationInfo info;
+						Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
+						if(!t.isNull())
+						{
+							UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
+									oldId,
+									signature->id(),
+									info.varianceLin,
+									info.varianceAng,
+									guess.prettyPrint().c_str(),
+									t.prettyPrint().c_str());
+							UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
+							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.varianceAng, info.varianceLin));
+
+							if(_optimizeFromGraphEnd)
+							{
+								// update all previous nodes
+								// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
+								// parameters just changed state, we should put back all poses without map correction.
+								Transform u = guess * t.inverse();
+								std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
+								UASSERT(jter!=_optimizedPoses.end());
+								Transform up = jter->second * u * jter->second.inverse();
+								Transform mapCorrectionInv = _mapCorrection.inverse();
+								for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+								{
+									iter->second = mapCorrectionInv * up * iter->second;
+								}
+							}
+						}
+						else
+						{
+							UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
+							if(info.varianceLin > 0 && info.varianceAng > 0)
+							{
+								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrt(info.varianceAng), sqrt(info.varianceLin)));
+							}
+						}
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
+					}
+				}
+				timeNeighborLinkRefining = timer.ticks();
+				ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
+
+				UASSERT(oldS->hasLink(signature->id()));
+				UASSERT(uContains(_optimizedPoses, oldId));
+
+				statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().at(signature->id()).transVariance());
+
+				newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
+				_mapCorrection = newPose * signature->getPose().inverse();
+				if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
+				{
+					UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
+							_mapCorrection.prettyPrint().c_str(),
+							newPose.prettyPrint().c_str(),
+							signature->getPose().prettyPrint().c_str());
 				}
 			}
 			else
 			{
-				//============================================================
+				UWARN("Neighbor link refining is activated but there are intermediate nodes, aborting refining...");
-				// Refine neighbor links
-				//============================================================
-				if(!signature->sensorData().laserScanCompressed().empty())
-				{
-					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
-					RegistrationInfo info;
-					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
-					if(!t.isNull())
-					{
-						UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
-								oldId,
-								signature->id(),
-								info.variance,
-								guess.prettyPrint().c_str(),
-								t.prettyPrint().c_str());
-						UASSERT(info.variance > 0.0);
-						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.variance, info.variance));
-
-						if(_optimizeFromGraphEnd)
-						{
-							// update all previous nodes
-							// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
-							// parameters just changed state, we should put back all poses without map correction.
-							Transform u = guess * t.inverse();
-							std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
-							UASSERT(jter!=_optimizedPoses.end());
-							Transform up = jter->second * u * jter->second.inverse();
-							Transform mapCorrectionInv = _mapCorrection.inverse();
-							for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-							{
-								iter->second = mapCorrectionInv * up * iter->second;
-							}
-						}
-					}
-					else
-					{
-						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
-						if(info.variance > 0)
-						{
-							double sqrtVar = sqrt(info.variance);
-							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrtVar, sqrtVar));
-						}
-					}
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
-				}
-			}
-			timeNeighborLinkRefining = timer.ticks();
-			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
-
-			UASSERT(oldS->hasLink(signature->id()));
-			UASSERT(uContains(_optimizedPoses, oldId));
-
-			newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
-			_mapCorrection = newPose * signature->getPose().inverse();
-			if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
-			{
-				UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
-						_mapCorrection.prettyPrint().c_str(),
-						newPose.prettyPrint().c_str(),
-						signature->getPose().prettyPrint().c_str());
 			}
 		}
 		else
@@ -1220,8 +1231,8 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(info.variance > 0.0);
+						UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.varianceAng, info.varianceLin)))
 						{
 							++proximityDetectionsInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1763,7 +1774,7 @@ bool Rtabmap::process(
 		//Compute transform if metric data are present
 		Transform transform;
 		RegistrationInfo info;
-		info.variance = 1.0f;
+		info.varianceLin = info.varianceAng = 1.0f;
 		if(_rgbdSlamMode)
 		{
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
@@ -1782,8 +1793,8 @@ bool Rtabmap::process(
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			UASSERT(info.variance > 0.0);
+			UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.variance, info.variance));
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.varianceAng, info.varianceLin));
 			if(!rejectedHypothesis)
 			{
 				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1877,8 +1888,8 @@ bool Rtabmap::process(
 						{
 							++localVisualPathsChecked;
 							RegistrationInfo info;
-							Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
+							// guess is null to make sure visual correspondences are globally computed
-							Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
+							Transform transform = _memory->computeTransform(nearestId, signature->id(), Transform(), &info);
 							if(!transform.isNull())
 							{
 								transform = transform.inverse();
@@ -1888,8 +1899,8 @@ bool Rtabmap::process(
 											signature->id(),
 											nearestId,
 											transform.prettyPrint().c_str());
-									UASSERT(info.variance > 0.0);
+									UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
-									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
+									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.varianceAng, info.varianceLin));
 									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 									if(loopClosureVisualInliers == 0)
@@ -2013,9 +2024,8 @@ bool Rtabmap::process(
 											}
 
 											// set Identify covariance for laser scan matching only
-											UASSERT(info.variance>0.0);
+											UASSERT(info.varianceLin>0.0 && info.varianceAng>0.0);
-											double sqrtVar = sqrt(info.variance);
+											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrt(info.varianceAng), sqrt(info.varianceLin), scanMatchingIds));
-											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
 											loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 											++proximityDetectionsAddedByICPOnly;
@@ -3397,6 +3407,19 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 	std::map<int, Signature> signatures;
 	this->getGraph(poses, links, true, true, &signatures);
 
+	//remove all invalid or intermediate nodes
+	for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
+	{
+		if(signatures.at(iter->first).getWeight() < 0)
+		{
+			poses.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
@@ -3442,8 +3465,8 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 						UINFO("Added new loop closure between %d and %d.", from, to);
 						addedLinks.insert(from);
 						addedLinks.insert(to);
-						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
+						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
-						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
+						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin));
 						UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
 					}
 				}
@@ -3512,7 +3535,7 @@ int Rtabmap::refineLinks()
 
 		if(!t.isNull())
 		{
-			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.variance, info.variance));
+			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.varianceAng, info.varianceLin));
 			UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
 		}
 	}

corelib/src/RtabmapThread.cpp

@@ -568,7 +568,8 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().variance>=9999 ||
+						odomEvent.info().varianceLin>=9999 ||
+						odomEvent.info().varianceAng>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
 						odomEvent.transVariance()>=9999))
 		{
@@ -581,13 +582,13 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		double maxRotVar = odomEvent.rotVariance();
 		double maxTransVar = odomEvent.transVariance();
 		// FIXME: should merge the transformations/variances like Link::merge();
-		if(maxRotVar > _rotVariance)
+		if(maxRotVar != 1.0f)
 		{
-			_rotVariance = maxRotVar;
+			_rotVariance += maxRotVar;
 		}
-		if(maxTransVar > _transVariance)
+		if(maxTransVar != 1.0f)
 		{
-			_transVariance = maxTransVar;
+			_transVariance += maxTransVar;
 		}
 
 		if(ignoreFrame && !_createIntermediateNodes)
@@ -617,7 +618,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
-			tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
+			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
 		}
 		else

corelib/src/SensorData.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
 
 namespace rtabmap
 {
@@ -736,6 +737,15 @@ void SensorData::uncompressDataConst(
 	}
 }
 
+void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors)
+{
+	UASSERT_MSG(keypoints3D.empty() || keypoints.size() == keypoints3D.size(), uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
+	UASSERT_MSG(descriptors.empty() || keypoints.size() == descriptors.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
+	_keypoints = keypoints;
+	_keypoints3D = keypoints3D;
+	_descriptors = descriptors;
+}
+
 long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
 	return _imageCompressed.total()*_imageCompressed.elemSize() +

corelib/src/Transform.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 
 #include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
@@ -208,6 +209,13 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 	z = this->z();
 }
 
+float Transform::getAngle(float x, float y, float z) const
+{
+	Eigen::Vector3f vA(x,y,z);
+	Eigen::Vector3f vB = this->toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+	return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+}
+
 float Transform::getNorm() const
 {
 	return uNorm(this->x(), this->y(), this->z());

corelib/src/util3d_motion_estimation.cpp

@@ -151,7 +151,7 @@ Transform estimateMotion3DTo2D(
 						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] < 100.0f)
+						if(errorSqrdDists[oi] < iter->second.x/100.0f)
 						{
 							++oi;
 						}

corelib/src/util3d_registration.cpp

@@ -353,13 +353,6 @@ Transform icpPointToPlane(
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);
 
-	if(icp2D)
-	{
-		pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>);
-		icp.setTransformationEstimation(est);
-	}
-
 	pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
 	icp.setTransformationEstimation(est);
@@ -377,7 +370,15 @@ Transform icpPointToPlane(
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
-	return Transform::fromEigen4f(icp.getFinalTransformation());
+	Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
+
+	if(icp2D)
+	{
+		// FIXME probably an estimation approach already 2D like in icp() version above exists.
+		t = t.to3DoF();
+	}
+
+	return t;
 }
 
 }

guilib/include/rtabmap/gui/MainWindow.h

@@ -245,7 +245,7 @@ private:
 	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
-	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
+	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth, double stamp = 0.0, int refId = -1);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceMenu();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -150,6 +150,7 @@ public:
 	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
+	int getOdomRegistrationApproach() const;
 	bool isOdomDisabled() const;
 	bool isGroundTruthAligned() const;
 
@@ -223,6 +224,7 @@ public:
 
 	bool isSourceDatabaseStampsUsed() const;
 	bool isSourceRGBDColorOnly() const;
+	bool isDepthFilteringAvailable() const;
 	QString getSourceDistortionModel() const;
 	bool isBilateralFiltering() const;
 	double getBilateralSigmaS() const;
@@ -307,6 +309,7 @@ private slots:
 	void selectSourceRGBDImagesPathRGB();
 	void selectSourceRGBDImagesPathDepth();
 	void selectSourceImagesPathScans();
+	void selectSourceImagesPathOdom();
 	void selectSourceImagesPathGt();
 	void selectSourceStereoImagesPathLeft();
 	void selectSourceStereoImagesPathRight();

guilib/src/CloudViewer.cpp

@@ -197,11 +197,16 @@ void CloudViewer::clear()
 	this->removeAllLines();
 	this->removeAllFrustums();
 	this->removeAllTexts();
-	this->clearTrajectory();
 	this->removeOccupancyGridMap();
 	this->removeOctomap();
 
 	this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
+	_lastPose.setNull();
+	if(_aLockCamera->isChecked() || _aFollowCamera->isChecked())
+	{
+		resetCamera();
+	}
+	this->clearTrajectory();
 }
 
 void CloudViewer::createMenu()

guilib/src/DatabaseViewer.cpp

@@ -3736,7 +3736,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				}
 				else if(localMaps_.find(ids[i]) != localMaps_.end())
 				{
-					localMaps.insert(*localMaps_.find(ids.at(i)));
+					if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty())
+					{
+						localMaps.insert(*localMaps_.find(ids.at(i)));
+					}
 				}
 				else
 				{
@@ -3745,6 +3748,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					cv::Mat ground, obstacles;
 					data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
 					localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
+					if(!ground.empty() || !obstacles.empty())
+					{
+						localMaps.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
+					}
 				}
 			}
 			//cleanup
@@ -4187,7 +4194,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 
 	if(!transform.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.variance, info.variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.varianceAng, info.varianceLin);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -4285,7 +4292,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 		
 		if(!t.isNull())
 		{
-			newLink = Link(from, to, Link::kUserClosure, t, info.variance, info.variance);
+			newLink = Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin);
 		}
 		else if(!silent)
 		{

guilib/src/MainWindow.cpp

@@ -553,14 +553,23 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("Odometry/ID/", false);
 	_ui->statsToolBox->updateStat("Odometry/Features/", false);
 	_ui->statsToolBox->updateStat("Odometry/Matches/", false);
+	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
 	_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
+	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
-	_ui->statsToolBox->updateStat("Odometry/StdDev/", false);
+	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
-	_ui->statsToolBox->updateStat("Odometry/Variance/", false);
+	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
+	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
+	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
 	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
+	_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
+	_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
+	_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
+	_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
+	_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
 	_ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
 	_ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
 	_ui->statsToolBox->updateStat("Odometry/Distance/m", false);
@@ -1293,15 +1302,24 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 
 	//Process info
 	_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/StdDev/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().variance), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
-	_ui->statsToolBox->updateStat("Odometry/Variance/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().variance, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceLin), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceLin, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceAng), _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceAng, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
 
 	float x=0.0f,y,z, roll,pitch,yaw;
@@ -1341,6 +1359,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		_ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
+		if(odom.info().interval > 0)
+		{
+			_ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
+		}
 	}
 
 	//cumulative pose
@@ -1689,7 +1711,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 
 			std::map<int, Transform> poses = stat.poses();
-			Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth);
+			Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth, stat.stamp(), stat.refImageId());
 			UDEBUG("time= %d ms", time.restart());
 
 			if(!_odometryReceived && poses.size())
@@ -1749,112 +1771,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 				UDEBUG("time= %d ms", time.restart());
 			}
-
-			// ground truth live statistics
-			if(poses.size() && groundTruth.size())
-			{
-				std::vector<float> translationalErrors(poses.size());
-				std::vector<float> rotationalErrors(poses.size());
-				int oi=0;
-				float sumTranslationalErrors = 0.0f;
-				float sumRotationalErrors = 0.0f;
-				float sumSqrdTranslationalErrors = 0.0f;
-				float sumSqrdRotationalErrors = 0.0f;
-				float radToDegree = 180.0f / M_PI;
-				float translational_min = 0.0f;
-				float translational_max = 0.0f;
-				float rotational_min = 0.0f;
-				float rotational_max = 0.0f;
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					std::map<int, Transform>::iterator jter = groundTruth.find(iter->first);
-					if(jter!=groundTruth.end())
-					{
-						Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-						Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
-						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-						rotationalErrors[oi] = a*radToDegree;
-						translationalErrors[oi] = iter->second.getDistance(jter->second);
-
-						sumTranslationalErrors+=translationalErrors[oi];
-						sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
-						sumRotationalErrors+=rotationalErrors[oi];
-						sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
-
-						if(oi == 0)
-						{
-							translational_min = translational_max = translationalErrors[oi];
-							rotational_min = rotational_max = rotationalErrors[oi];
-						}
-						else
-						{
-							if(translationalErrors[oi] < translational_min)
-							{
-								translational_min = translationalErrors[oi];
-							}
-							else if(translationalErrors[oi] > translational_max)
-							{
-								translational_max = translationalErrors[oi];
-							}
-
-							if(rotationalErrors[oi] < rotational_min)
-							{
-								rotational_min = rotationalErrors[oi];
-							}
-							else if(rotationalErrors[oi] > rotational_max)
-							{
-								rotational_max = rotationalErrors[oi];
-							}
-						}
-
-						++oi;
-					}
-				}
-				translationalErrors.resize(oi);
-				rotationalErrors.resize(oi);
-				if(oi)
-				{
-					float total = float(oi);
-					float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
-					float translational_mean = sumTranslationalErrors/total;
-					float translational_median = translationalErrors[oi/2];
-					float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
-
-					float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
-					float rotational_mean = sumRotationalErrors/total;
-					float rotational_median = rotationalErrors[oi/2];
-					float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
-
-					UINFO("translational_rmse=%f", translational_rmse);
-					UINFO("translational_mean=%f", translational_mean);
-					UINFO("translational_median=%f", translational_median);
-					UINFO("translational_std=%f", translational_std);
-					UINFO("translational_min=%f", translational_min);
-					UINFO("translational_max=%f", translational_max);
-
-					UINFO("rotational_rmse=%f", rotational_rmse);
-					UINFO("rotational_mean=%f", rotational_mean);
-					UINFO("rotational_median=%f", rotational_median);
-					UINFO("rotational_std=%f", rotational_std);
-					UINFO("rotational_min=%f", rotational_min);
-					UINFO("rotational_max=%f", rotational_max);
-
-					_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_rmse, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_mean, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_median, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_std, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_min, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_max, _preferencesDialog->isCacheSavedInFigures());
-
-					_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_mean, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_median, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_std, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_min, _preferencesDialog->isCacheSavedInFigures());
-					_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_max, _preferencesDialog->isCacheSavedInFigures());
-				}
-			}
-			UDEBUG("time= %d ms", time.restart());
 		}
 
 		if( _ui->graphicsView_graphView->isVisible())
@@ -2990,50 +2906,176 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
 
 Transform MainWindow::alignPosesToGroundTruth(
 		std::map<int, Transform> & poses,
-		const std::map<int, Transform> & groundTruth)
+		const std::map<int, Transform> & groundTruth,
+		double stamp,
+		int refId)
 {
 	Transform t = Transform::getIdentity();
-	if(groundTruth.size() && poses.size() && _preferencesDialog->isGroundTruthAligned())
+	if(groundTruth.size() && poses.size())
 	{
-		unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
+		if(_preferencesDialog->isGroundTruthAligned())
-		pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
-		cloud1.resize(maxSize);
-		cloud2.resize(maxSize);
-		int oi = 0;
-		int idFirst = 0;
-		for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
 		{
-			std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+			unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
-			if(iter2!=poses.end())
+			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+			cloud1.resize(maxSize);
+			cloud2.resize(maxSize);
+			int oi = 0;
+			int idFirst = 0;
+			for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
 			{
-				if(oi==0)
+				std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+				if(iter2!=poses.end())
 				{
-					idFirst = iter->first;
+					if(oi==0)
+					{
+						idFirst = iter->first;
+					}
+					cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+					cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
 				}
-				cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-				cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
 			}
+
+			if(oi>5)
+			{
+				cloud1.resize(oi);
+				cloud2.resize(oi);
+
+				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+			}
+			else if(idFirst)
+			{
+				t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+			}
+			if(!t.isIdentity())
+			{
+				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					iter->second = t * iter->second;
+				}
+			}
+			UDEBUG("t=%s", t.prettyPrint().c_str());
 		}
 
-		if(oi>5)
+		// ground truth live statistics
+		std::vector<float> translationalErrors(poses.size());
+		std::vector<float> rotationalErrors(poses.size());
+		int oi=0;
+		float sumTranslationalErrors = 0.0f;
+		float sumRotationalErrors = 0.0f;
+		float sumSqrdTranslationalErrors = 0.0f;
+		float sumSqrdRotationalErrors = 0.0f;
+		float radToDegree = 180.0f / M_PI;
+		float translational_min = 0.0f;
+		float translational_max = 0.0f;
+		float rotational_min = 0.0f;
+		float rotational_max = 0.0f;
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			cloud1.resize(oi);
+			std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
-			cloud2.resize(oi);
+			if(jter!=groundTruth.end())
-		
-			t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
-		}
-		else if(idFirst)
-		{
-			t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
-		}
-		if(!t.isIdentity())
-		{
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
-				iter->second = t * iter->second;
+				Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+				double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+				rotationalErrors[oi] = a*radToDegree;
+				translationalErrors[oi] = iter->second.getDistance(jter->second);
+
+				sumTranslationalErrors+=translationalErrors[oi];
+				sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
+				sumRotationalErrors+=rotationalErrors[oi];
+				sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
+
+				if(oi == 0)
+				{
+					translational_min = translational_max = translationalErrors[oi];
+					rotational_min = rotational_max = rotationalErrors[oi];
+				}
+				else
+				{
+					if(translationalErrors[oi] < translational_min)
+					{
+						translational_min = translationalErrors[oi];
+					}
+					else if(translationalErrors[oi] > translational_max)
+					{
+						translational_max = translationalErrors[oi];
+					}
+
+					if(rotationalErrors[oi] < rotational_min)
+					{
+						rotational_min = rotationalErrors[oi];
+					}
+					else if(rotationalErrors[oi] > rotational_max)
+					{
+						rotational_max = rotationalErrors[oi];
+					}
+				}
+
+				++oi;
+			}
+		}
+		translationalErrors.resize(oi);
+		rotationalErrors.resize(oi);
+		if(oi)
+		{
+			float total = float(oi);
+			float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
+			float translational_mean = sumTranslationalErrors/total;
+			float translational_median = translationalErrors[oi/2];
+			float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
+
+			float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
+			float rotational_mean = sumRotationalErrors/total;
+			float rotational_median = rotationalErrors[oi/2];
+			float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
+
+			UINFO("translational_rmse=%f", translational_rmse);
+			UINFO("translational_mean=%f", translational_mean);
+			UINFO("translational_median=%f", translational_median);
+			UINFO("translational_std=%f", translational_std);
+			UINFO("translational_min=%f", translational_min);
+			UINFO("translational_max=%f", translational_max);
+
+			UINFO("rotational_rmse=%f", rotational_rmse);
+			UINFO("rotational_mean=%f", rotational_mean);
+			UINFO("rotational_median=%f", rotational_median);
+			UINFO("rotational_std=%f", rotational_std);
+			UINFO("rotational_min=%f", rotational_min);
+			UINFO("rotational_max=%f", rotational_max);
+
+			if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0))
+			{
+				_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
+
+				_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
+			}
+			else
+			{
+				_ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/translational_max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
+
+				_ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
+				_ui->statsToolBox->updateStat("GT/rotational_max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
 			}
 		}
-		UDEBUG("t=%s", t.prettyPrint().c_str());
 	}
 	return t;
 }
@@ -4191,7 +4233,7 @@ void MainWindow::startDetection()
 			_preferencesDialog->getSourceScanFromDepthMaxDepth(),
 			_preferencesDialog->getSourceScanVoxelSize(),
 			_preferencesDialog->getSourceScanNormalsK());
-	if(_preferencesDialog->getSourceType() == PreferencesDialog::kSrcRGBD)
+	if(_preferencesDialog->isDepthFilteringAvailable())
 	{
 		if(_preferencesDialog->isBilateralFiltering())
 		{
@@ -4234,7 +4276,12 @@ void MainWindow::startDetection()
 
 			if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
 			{
-				Odometry * odom = Odometry::create(parameters);
+				ParametersMap odomParameters = parameters;
+				if(_preferencesDialog->getOdomRegistrationApproach() < 3)
+				{
+					uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
+				}
+				Odometry * odom = Odometry::create(odomParameters);
 				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
 
 				UEventsManager::addHandler(_odomThread);
@@ -4537,7 +4584,6 @@ void MainWindow::postProcessing()
 	int detectLoopClosureIterations = _postProcessingDialog->iterations();
 	bool sba = _postProcessingDialog->isSBA();
 	int sbaIterations = _postProcessingDialog->sbaIterations();
-	double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
 	double sbaVariance = _postProcessingDialog->sbaVariance();
 	Optimizer::Type sbaType = _postProcessingDialog->sbaType();
 
@@ -4689,7 +4735,7 @@ void MainWindow::postProcessing()
 									UINFO("Added new loop closure between %d and %d.", from, to);
 									addedLinks.insert(from);
 									addedLinks.insert(to);
-									_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
+									_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin)));
 									++loopClosuresAdded;
 									_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 									QApplication::processEvents();
@@ -4785,7 +4831,7 @@ void MainWindow::postProcessing()
 
 						if(!transform.isNull())
 						{
-							Link newLink(from, to, iter->second.type(), transform, info.variance, info.variance);
+							Link newLink(from, to, iter->second.type(), transform, info.varianceAng, info.varianceLin);
 							iter->second = newLink;
 						}
 						else
@@ -4846,7 +4892,6 @@ void MainWindow::postProcessing()
 
 		ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
 		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
-		uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
 		uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
 		Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
 		std::map<int, Transform>  newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
@@ -6272,7 +6317,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
 		_ui->actionDump_the_memory->setEnabled(true);
 		_ui->actionDump_the_prediction_matrix->setEnabled(true);
-		_ui->actionDelete_memory->setEnabled(true);
+		_ui->actionDelete_memory->setEnabled(_openedDatabasePath.isEmpty());
 		_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
 		_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
 		_ui->actionGenerate_map->setEnabled(true);

guilib/src/PostProcessingDialog.cpp

@@ -72,7 +72,6 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
 
 	connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
-	connect(_ui->sba_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
 
@@ -104,7 +103,6 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
 	settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
 	settings.setValue("sba", this->isSBA());
 	settings.setValue("sba_iterations", this->sbaIterations());
-	settings.setValue("sba_epsilon", this->sbaEpsilon());
 	settings.setValue("sba_type", this->sbaType());
 	settings.setValue("sba_variance", this->sbaVariance());
 	if(!group.isEmpty())
@@ -127,7 +125,6 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 	this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
 	this->setSBA(settings.value("sba", this->isSBA()).toBool());
 	this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
-	this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
 	this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
 	this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
 	if(!group.isEmpty())
@@ -139,14 +136,13 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
 void PostProcessingDialog::restoreDefaults()
 {
 	setDetectMoreLoopClosures(true);
-	setClusterRadius(0.5);
+	setClusterRadius(1);
 	setClusterAngle(30);
-	setIterations(3);
+	setIterations(5);
 	setRefineNeighborLinks(false);
 	setRefineLoopClosureLinks(false);
 	setSBA(false);
 	setSBAIterations(20);
-	setSBAEpsilon(0.0);
 	setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
 	setSBAVariance(1.0);
 }
@@ -196,10 +192,6 @@ int PostProcessingDialog::sbaIterations() const
 {
 	return _ui->sba_iterations->value();
 }
-double PostProcessingDialog::sbaEpsilon() const
-{
-	return _ui->sba_epsilon->value();
-}
 double PostProcessingDialog::sbaVariance() const
 {
 	return _ui->sba_variance->value();
@@ -242,10 +234,6 @@ void PostProcessingDialog::setSBAIterations(int iterations)
 {
 	_ui->sba_iterations->setValue(iterations);
 }
-void PostProcessingDialog::setSBAEpsilon(double epsilon)
-{
-	_ui->sba_epsilon->setValue(epsilon);
-}
 void PostProcessingDialog::setSBAVariance(double variance)
 {
 	_ui->sba_variance->setValue(variance);

guilib/src/PostProcessingDialog.h

@@ -59,7 +59,6 @@ public:
 	bool isRefineLoopClosureLinks() const;
 	bool isSBA() const;
 	int sbaIterations() const;
-	double sbaEpsilon() const;
 	double sbaVariance() const;
 	Optimizer::Type sbaType() const;
 
@@ -72,7 +71,6 @@ public:
 	void setRefineLoopClosureLinks(bool on);
 	void setSBA(bool on);
 	void setSBAIterations(int iterations);
-	void setSBAEpsilon(double epsilon);
 	void setSBAVariance(double variance);
 	void setSBAType(Optimizer::Type type);
 

guilib/src/PreferencesDialog.cpp

@@ -180,6 +180,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->groupBox_fast_opencv2->setEnabled(false);
 #endif
 
+	_ui->comboBox_cameraImages_odomFormat->setItemData(4, 0, Qt::UserRole - 1);
 	_ui->comboBox_cameraImages_gtFormat->setItemData(4, 0, Qt::UserRole - 1);
 	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 	{
@@ -255,6 +256,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
+	// in case we change the ui, we should not forget to change stuff related to this parameter
+	UASSERT(_ui->odom_registration->count() == 4);
+
 	// Default Driver
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
@@ -291,6 +295,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
 	// Cloud rendering panel
@@ -474,6 +479,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_cameraRGBDImages_path_rgb, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathRGB()));
 	connect(_ui->toolButton_cameraRGBDImages_path_depth, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathDepth()));
 	connect(_ui->toolButton_cameraImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathScans()));
+	connect(_ui->toolButton_cameraImages_odom, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathOdom()));
 	connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -485,6 +491,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraImages_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_cameraImages_odom, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	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->groupBox_depthFromScan, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
@@ -726,6 +734,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
 	_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());
 	_ui->doubleSpinBox_g2o_pixelVariance->setObjectName(Parameters::kg2oPixelVariance().c_str());
+	_ui->doubleSpinBox_g2o_robustKernelDelta->setObjectName(Parameters::kg2oRobustKernelDelta().c_str());
+	_ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
@@ -833,6 +843,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
 	_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
+	_ui->odom_VisKeyFrameThr->setObjectName(Parameters::kOdomVisKeyFrameThr().c_str());
 	_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
 	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
 	_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
@@ -1395,6 +1406,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
 		_ui->spinBox_cameraImages_scanDownsampleStep->setValue(1);
 		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
+		_ui->lineEdit_cameraImages_odom->setText("");
+		_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
 		_ui->lineEdit_cameraImages_gt->setText("");
 		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
 
@@ -1477,6 +1490,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		{
 			this->setupKpRoiPanel();
 		}
+
+		if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
+		{
+			_ui->odom_registration->setCurrentIndex(3);
+		}
 	}
 }
 
@@ -1596,6 +1614,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
+	_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
 	_ui->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
 
 	for(int i=0; i<2; ++i)
@@ -1766,6 +1785,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
 	_ui->spinBox_cameraImages_scanDownsampleStep->setValue(settings.value("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value()).toInt());
 	_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
+	_ui->lineEdit_cameraImages_odom->setText(settings.value("odom_path", _ui->lineEdit_cameraImages_odom->text()).toString());
+	_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());
 	settings.endGroup(); // images
@@ -1975,6 +1996,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("odomQualityThr",       _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView",   _ui->checkBox_posteriorGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
+	settings.setValue("odomRegistration",     _ui->odom_registration->currentIndex());
 	settings.setValue("gtAlign",              _ui->checkbox_groundTruthAlign->isChecked());
 
 	for(int i=0; i<2; ++i)
@@ -2147,6 +2169,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("scan_max_pts",        _ui->spinBox_cameraImages_max_scan_pts->value());
 	settings.setValue("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value());
 	settings.setValue("scan_voxel_size",     _ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
+	settings.setValue("odom_path",           _ui->lineEdit_cameraImages_odom->text());
+	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.endGroup(); // images
@@ -2305,7 +2329,9 @@ bool PreferencesDialog::validateForm()
 				   "with cvsba. Bundle adjustment is disabled."));
 		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
 	}
-	if(_ui->odom_f2m_bundleStrategy->currentIndex() > 0 && _ui->loopClosure_correspondencesType->currentIndex() == 1)
+	if(_ui->odom_strategy->currentIndex() == 0 && // F2M
+		_ui->odom_f2m_bundleStrategy->currentIndex() > 0 &&
+		_ui->loopClosure_correspondencesType->currentIndex() == 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Odometry local bundle adjustment optimization cannot be used at the same time than Optical Flow correspondences "
@@ -2916,6 +2942,30 @@ void PreferencesDialog::selectSourceRGBDImagesPathDepth()
 	}
 }
 
+void PreferencesDialog::selectSourceImagesPathOdom()
+{
+	QString dir = _ui->lineEdit_cameraImages_odom->text();
+	if(dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Odometry (*.txt *.log *.toro *.g2o)"));
+	if(path.size())
+	{
+		QStringList list;
+		for(int i=0; i<_ui->comboBox_cameraImages_odomFormat->count(); ++i)
+		{
+			list.push_back(_ui->comboBox_cameraImages_odomFormat->itemText(i));
+		}
+		QString item = QInputDialog::getItem(this, tr("Odometry Format"), tr("Format:"), list, _ui->comboBox_cameraImages_odomFormat->currentIndex(), false);
+		if(!item.isEmpty())
+		{
+			_ui->lineEdit_cameraImages_odom->setText(path);
+			_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(_ui->comboBox_cameraImages_odomFormat->findText(item));
+		}
+	}
+}
+
 void PreferencesDialog::selectSourceImagesPathGt()
 {
 	QString dir = _ui->lineEdit_cameraImages_gt->text();
@@ -2931,7 +2981,7 @@ void PreferencesDialog::selectSourceImagesPathGt()
 		{
 			list.push_back(_ui->comboBox_cameraImages_gtFormat->itemText(i));
 		}
-		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
+		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, _ui->comboBox_cameraImages_gtFormat->currentIndex(), false);
 		if(!item.isEmpty())
 		{
 			_ui->lineEdit_cameraImages_gt->setText(path);
@@ -3704,6 +3754,11 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
 			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB));
 
+	_ui->groupBox_depthImageFiltering->setEnabled(
+				_ui->comboBox_sourceType->currentIndex() == 0 || // RGBD
+				_ui->comboBox_sourceType->currentIndex() == 3);  // Database
+	_ui->groupBox_depthImageFiltering->setVisible(_ui->groupBox_depthImageFiltering->isEnabled());
+
 	//_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
 
 	_ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
@@ -3787,6 +3842,10 @@ bool PreferencesDialog::isOdomDisabled() const
 {
 	return _ui->checkbox_odomDisabled->isChecked();
 }
+int PreferencesDialog::getOdomRegistrationApproach() const
+{
+	return _ui->odom_registration->currentIndex();
+}
 bool PreferencesDialog::isGroundTruthAligned() const
 {
 	return _ui->checkbox_groundTruthAlign->isChecked();
@@ -4135,6 +4194,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
 }
+bool PreferencesDialog::isDepthFilteringAvailable() const
+{
+	return _ui->groupBox_depthImageFiltering->isEnabled();
+}
 QString PreferencesDialog::getSourceDistortionModel() const
 {
 	return _ui->lineEdit_source_distortionModel->text();
@@ -4277,6 +4340,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 		((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)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -4321,6 +4385,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 		((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)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -4421,6 +4486,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked() && !useRawImages);
 
 		((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
+		((CameraImages*)camera)->setOdometryPath(
+				_ui->lineEdit_cameraImages_odom->text().toStdString(),
+				_ui->comboBox_cameraImages_odomFormat->currentIndex());
 		((CameraImages*)camera)->setGroundTruthPath(
 				_ui->lineEdit_cameraImages_gt->text().toStdString(),
 				_ui->comboBox_cameraImages_gtFormat->currentIndex());
@@ -4643,6 +4711,10 @@ void PreferencesDialog::testOdometry()
 	}
 
 	ParametersMap parameters = this->getAllParameters();
+	if(getOdomRegistrationApproach() < 3)
+	{
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach())));
+	}
 	Odometry * odometry = Odometry::create(parameters);
 
 	OdometryThread odomThread(
@@ -4672,7 +4744,7 @@ void PreferencesDialog::testOdometry()
 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 			_ui->spinBox_cameraImages_scanNormalsK->value());
-	if(this->getSourceType() == PreferencesDialog::kSrcRGBD)
+	if(isDepthFilteringAvailable())
 	{
 		if(_ui->groupBox_bilateral->isChecked())
 		{
@@ -4720,7 +4792,7 @@ void PreferencesDialog::testCamera()
 				_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 				_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 				_ui->spinBox_cameraImages_scanNormalsK->value());
-		if(this->getSourceType() == PreferencesDialog::kSrcRGBD)
+		if(isDepthFilteringAvailable())
 		{
 			if(_ui->groupBox_bilateral->isChecked())
 			{

guilib/src/ui/DatabaseViewer.ui

@@ -52,8 +52,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>194</width>
+           <width>253</width>
-           <height>196</height>
+           <height>208</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -210,8 +210,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>194</width>
+           <width>253</width>
-           <height>196</height>
+           <height>208</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -484,7 +484,7 @@
      <x>0</x>
      <y>0</y>
      <width>1285</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -994,8 +994,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>424</width>
+          <width>324</width>
-          <height>186</height>
+          <height>188</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1130,8 +1130,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>424</width>
+          <width>280</width>
-          <height>535</height>
+          <height>584</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1447,8 +1447,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>437</width>
+          <width>201</width>
-          <height>162</height>
+          <height>126</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1547,8 +1547,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>424</width>
+          <width>333</width>
-          <height>198</height>
+          <height>256</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2014,12 +2014,12 @@
   </action>
   <action name="actionView_3D_laser_scans">
    <property name="text">
-    <string>View 3D laser scans...</string>
+    <string>View 2D-3D laser scans...</string>
    </property>
   </action>
   <action name="actionExport_3D_laser_scans_ply_pcd">
    <property name="text">
-    <string>Export 3D laser scans (*.ply *.pcd) ...</string>
+    <string>Export 2D-3D laser scans (*.ply *.pcd) ...</string>
    </property>
   </action>
   <action name="actionRegenerate_local_grid_maps">

guilib/src/ui/mainWindow.ui

@@ -1141,7 +1141,7 @@
   </action>
   <action name="actionExport_2D_scans_ply_pcd">
    <property name="text">
-    <string>Export 2D scans (*.ply *.pcd)...</string>
+    <string>Export 2D-3D scans (*.ply *.pcd)...</string>
    </property>
   </action>
   <action name="actionView_scans">

guilib/src/ui/postProcessingDialog.ui

@@ -194,42 +194,6 @@
           </property>
          </widget>
         </item>
-        <item row="0" column="0">
-         <widget class="QComboBox" name="comboBox_sbaType">
-          <item>
-           <property name="text">
-            <string>g2o</string>
-           </property>
-          </item>
-          <item>
-           <property name="text">
-            <string>cvsba</string>
-           </property>
-          </item>
-         </widget>
-        </item>
-        <item row="2" column="0">
-         <widget class="QDoubleSpinBox" name="sba_epsilon">
-          <property name="suffix">
-           <string/>
-          </property>
-          <property name="decimals">
-           <number>4</number>
-          </property>
-          <property name="minimum">
-           <double>0.000000000000000</double>
-          </property>
-          <property name="maximum">
-           <double>1.000000000000000</double>
-          </property>
-          <property name="singleStep">
-           <double>0.000100000000000</double>
-          </property>
-          <property name="value">
-           <double>0.000100000000000</double>
-          </property>
-         </widget>
-        </item>
         <item row="1" column="1">
          <widget class="QLabel" name="label_8">
           <property name="text">
@@ -241,15 +205,37 @@
          </widget>
         </item>
         <item row="2" column="1">
-         <widget class="QLabel" name="label_9">
+         <widget class="QLabel" name="label_variance">
           <property name="text">
-           <string>Stop optimizing when the error improvement is less than this value.</string>
+           <string>Pixel variance used by g2o.</string>
           </property>
           <property name="wordWrap">
            <bool>true</bool>
           </property>
          </widget>
         </item>
+        <item row="2" column="0">
+         <widget class="QDoubleSpinBox" name="sba_variance">
+          <property name="suffix">
+           <string/>
+          </property>
+          <property name="decimals">
+           <number>2</number>
+          </property>
+          <property name="minimum">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="maximum">
+           <double>999.000000000000000</double>
+          </property>
+          <property name="singleStep">
+           <double>0.010000000000000</double>
+          </property>
+          <property name="value">
+           <double>1.000000000000000</double>
+          </property>
+         </widget>
+        </item>
         <item row="0" column="1">
          <widget class="QLabel" name="label_10">
           <property name="text">
@@ -260,36 +246,18 @@
           </property>
          </widget>
         </item>
-        <item row="3" column="1">
+        <item row="0" column="0">
-         <widget class="QLabel" name="label_variance">
+         <widget class="QComboBox" name="comboBox_sbaType">
-          <property name="text">
+          <item>
-           <string>Pixel variance used by g2o.</string>
+           <property name="text">
-          </property>
+            <string>g2o</string>
-          <property name="wordWrap">
+           </property>
-           <bool>true</bool>
+          </item>
-          </property>
+          <item>
-         </widget>
+           <property name="text">
-        </item>
+            <string>cvsba</string>
-        <item row="3" column="0">
+           </property>
-         <widget class="QDoubleSpinBox" name="sba_variance">
+          </item>
-          <property name="suffix">
-           <string/>
-          </property>
-          <property name="decimals">
-           <number>4</number>
-          </property>
-          <property name="minimum">
-           <double>0.000000000000000</double>
-          </property>
-          <property name="maximum">
-           <double>10.000000000000000</double>
-          </property>
-          <property name="singleStep">
-           <double>0.010000000000000</double>
-          </property>
-          <property name="value">
-           <double>1.000000000000000</double>
-          </property>
          </widget>
         </item>
        </layout>