Refactored Odometry class:

-new odometry parameters -new Optical flow strategy -Stereo data support Refactored SensorData class to support stereo images Updated default parameters (mostly odometry ones) util3d: new methods to handle/reconstruct 3D clouds from disparity image / stereo images PreferencesDialog: added Odometry/BOW and Odometry/OpticalFLow panels. DatabaseViewer: fixed a crash when database is empty. Added cloud reconstruction of stereo images if saved in database Camera: added 1 second delay to avoid dark images at the starting git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1849 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-01 17:10:26 +08:00 · 2014-10-13 19:10:22 +00:00
parent a3f7415821
commit 8b67633b34
25 changed files with 2108 additions and 498 deletions
--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -48,29 +48,6 @@ namespace gpu {

 namespace rtabmap {

-void RTABMAP_EXP filterKeypointsByDepth(
-			std::vector<cv::KeyPoint> & keypoints,
-			const cv::Mat & depth,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			float maxDepth);
-void RTABMAP_EXP filterKeypointsByDepth(
-		std::vector<cv::KeyPoint> & keypoints,
-		cv::Mat & descriptors,
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		float maxDepth);
-
-void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
-void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
-
-cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
-
 // Feature2D
 class RTABMAP_EXP Feature2D {
 public:
@@ -84,6 +61,23 @@ public:
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7};

+	static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
+	static void filterKeypointsByDepth(
+				std::vector<cv::KeyPoint> & keypoints,
+				const cv::Mat & depth,
+				float maxDepth);
+	static void filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			const cv::Mat & depth,
+			float maxDepth);
+
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, 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);
+
 public:
 	virtual ~Feature2D() {}

--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -168,10 +168,6 @@ public:
 	void extractKeypointsAndDescriptors(
 			const cv::Mat & image,
 			const cv::Mat & depth,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
 			std::vector<cv::KeyPoint> & keypoints,
 			cv::Mat & descriptors) const;

--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -50,6 +50,8 @@ class UTimer;

 namespace rtabmap {

+class Feature2D;
+
 class RTABMAP_EXP Odometry
 {
 public:
@@ -62,29 +64,31 @@ public:
 	//getters
 	const Transform & getPose() const {return _pose;}
 	int getMaxFeatures() const  {return _maxFeatures;}
+	const std::string & getRoiRatios() const {return _roiRatios;}
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
-	float getWordsRatio() const {return _wordsRatio;}
+	int getRefineIterations() const {return _refineIterations;}
+	float getFeaturesRatio() const {return _featuresRatio;}
 	float getMaxDepth() const {return _maxDepth;}
 	float geLinearUpdate() const {return _linearUpdate;}
 	float getAngularUpdate() const {return _angularUpdate;}
-	int getLocalHistoryMaxSize() const {return _localHistoryMaxSize;}

 private:
 	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;

 private:
 	int _maxFeatures;
+	std::string _roiRatios;
 	int _minInliers;
 	float _inlierDistance;
 	int _iterations;
-	float _wordsRatio;
+	int _refineIterations;
+	float _featuresRatio;
 	float _maxDepth;
 	float _linearUpdate;
 	float _angularUpdate;
 	int _resetCountdown;
-	int _localHistoryMaxSize;
 	Transform _pose;
 	int _resetCurrentCount;

@@ -108,10 +112,51 @@ private:
 	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);

 private:
+	//Parameters
+	int _localHistoryMaxSize;
+
 	Memory * _memory;
 	std::multimap<int, pcl::PointXYZ> localMap_;
 };

+class RTABMAP_EXP OdometryOpticalFlow : public Odometry
+{
+public:
+	OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryOpticalFlow();
+
+	virtual void reset();
+
+	const cv::Mat & getLastFrame() const {return lastFrame_;}
+	const std::vector<cv::Point2f> & getLastCorners() const {return lastCorners_;}
+	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return lastCorners3D_;}
+
+	cv::Mat imgMatches_;
+
+private:
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
+	Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
+private:
+	//Parameters:
+	int flowWinSize_;
+	int flowIterations_;
+	double flowEps_;
+	int flowMaxLevel_;
+
+	int subPixWinSize_;
+	int subPixIterations_;
+	double subPixEps_;
+
+	Feature2D * feature2D_;
+
+	cv::Mat lastFrame_;
+	cv::Mat lastRightFrame_;
+	std::vector<cv::Point2f> lastCorners_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr lastCorners3D_;
+	Transform savedLastRefFrameTransform_;
+};
+
 class RTABMAP_EXP OdometryICP : public Odometry
 {
 public:
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -204,7 +204,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");

-	RTABMAP_PARAM(GFTT, MaxCorners, int, 1000, "");
+	RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
 	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 1, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
@@ -247,29 +247,43 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
-	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, true,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");

 	// Local loop closure detection
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, true, 	"Detection over all locations in STM.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")

 	// Odometry
-	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
+	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(Odom, LinearUpdate,           float, 0.0, 	"Min linear displacement to update odometry.");
 	RTABMAP_PARAM(Odom, AngularUpdate,          float, 0.0, 	"Min angular displacement to update odometry.");
-	RTABMAP_PARAM(Odom, MaxWords,               int, 0, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, MaxFeatures,            int, 0, 		"0 no limits.");
 	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.01, 	"Maximum distance for visual word correspondences.");
-	RTABMAP_PARAM(Odom, MinInliers,             int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(Odom, MaxDepth,               float, 5.0, 	"Max depth of the words (0 means no limit).");
-	RTABMAP_PARAM(Odom, WordsRatio,             float, 0.5, 	"Minmum ratio of keypoints between the current image and the last image to compute odometry.");
+	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(Odom, Iterations,             int, 30, 		"Maximum iterations to compute the transform from visual words.");
+	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
-	RTABMAP_PARAM(Odom, LocalHistory,           int, 0,         "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
-	RTABMAP_PARAM(Odom, NearestNeighbor,        int, 1, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
-	RTABMAP_PARAM(Odom, NNDR,                   float, 0.7,     "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+	RTABMAP_PARAM(Odom, RefineIterations,       int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Odom, FeaturesRatio,          float, 0.5, 	"Minimum ratio of keypoints between the current image and the last image to compute odometry.");
+
+	// Odometry Bag-of-words
+	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
+	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
+
+	// Odometry Optical Flow
+	RTABMAP_PARAM(OdomFlow, WinSize,               int, 9,       "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, Iterations,            int, 20,       "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.02,  "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 4,        "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
+	RTABMAP_PARAM(OdomFlow, SubPixIterations,      int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(OdomFlow, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");

 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
--- a/corelib/include/rtabmap/core/SensorData.h
+++ b/corelib/include/rtabmap/core/SensorData.h
@@ -47,9 +47,9 @@ public:

 	// Metric constructor
 	SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -58,10 +58,10 @@ public:

 	// Metric constructor + 2d depth
 	SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  const cv::Mat & depth2d,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -78,14 +78,18 @@ public:
 	const cv::Mat & image() const {return _image;}
 	int id() const {return _id;};

-	bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
+	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
 	void setPose(const Transform & pose) {_pose = pose;}
-	const cv::Mat & depth() const {return _depth;}
+	cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
+	cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
+	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
 	const cv::Mat & depth2d() const {return _depth2d;}
-	float depthFx() const {return _fx;}
-	float depthFy() const {return _fy;}
-	float depthCx() const {return _cx;}
-	float depthCy() const {return _cy;}
+	float fx() const {return _fx;}
+	float fy() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_fyOrBaseline:0;}
+	float cx() const {return _cx;}
+	float cy() const {return _cy;}
+	float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;}
+	float fyOrBaseline() const {return _fyOrBaseline;}
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}

@@ -102,10 +106,10 @@ private:
 	int _id;

 	// Metric stuff
-	cv::Mat _depth;
+	cv::Mat _depthOrRightImage;
 	cv::Mat _depth2d;
 	float _fx;
-	float _fy;
+	float _fyOrBaseline;
 	float _cx;
 	float _cy;
 	Transform _pose;
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -186,6 +186,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation = 1);

+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDisparity,
+		float cx, float cy,
+		float fx, float baseline,
+		int decimation);
+
+cv::Mat RTABMAP_EXP disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage);
+
+pcl::PointXYZ RTABMAP_EXP projectDisparityTo3d(
+		const cv::Point2f & pt,
+		float disparity,
+		float cx, float cy, float fx, float baseline);
+
+cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
+		float cx, float cy, float fx, float baseline,
+		int type = CV_32FC1);
+
 cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);

@@ -242,7 +262,10 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
 		double inlierThreshold = 0.02,
 		int iterations = 100,
-		int * inliers = 0);
+		bool refineModel = false,
+		double refineModelSigma = 3.0,
+		int refineModelIterations = 10,
+		std::vector<int> * inliers = 0);

 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -92,6 +92,9 @@ bool CameraThread::init()
 		{
 			return _camera->init();
 		}
+
+		// Added sleep time to ignore first frames (which are darker)
+		uSleep(1000);
 	}
 	else
 	{
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -2025,7 +2025,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,

 	if(uStrNumCmp(_version, "0.7.0") < 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fy);
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 	else
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -46,41 +46,39 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

-void filterKeypointsByDepth(
+void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		float maxDepth)
 {
 	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, descriptors, depth, fx, fy, cx, cy, maxDepth);
+	filterKeypointsByDepth(keypoints, descriptors, depth, maxDepth);
 }

-void filterKeypointsByDepth(
+void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		float maxDepth)
 {
-	if(!depth.empty() && fx > 0.0f && fy > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
+	if(!depth.empty() && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
 		std::vector<int> indexes(keypoints.size(), 0);
 		int oi=0;
+		bool isInMM = depth.type() == CV_16UC1;
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy, true);
-			if(uIsFinite(pt.z) && pt.z < maxDepth)
+			int u = int(keypoints[i].pt.x+0.5f);
+			int v = int(keypoints[i].pt.y+0.5f);
+			if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
 			{
-				output[oi++] = keypoints[i];
-				indexes[i] = 1;
+				float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
+				if(d!=0.0f && uIsFinite(d) && d < maxDepth)
+				{
+					output[oi++] = keypoints[i];
+					indexes[i] = 1;
+				}
 			}
 		}
 		output.resize(oi);
@@ -116,15 +114,15 @@ void filterKeypointsByDepth(
 	}
 }

-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
 {
 	cv::Mat descriptors;
 	limitKeypoints(keypoints, descriptors, maxKeypoints);
 }

-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
 {
-	UASSERT((int)keypoints.size() == descriptors.rows || descriptors.rows == 0);
+	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
@@ -173,7 +171,39 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 	}
 }

-cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
+cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
+{
+	std::list<std::string> strValues = uSplit(roiRatios, ' ');
+	if(strValues.size() != 4)
+	{
+		UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
+	}
+	else
+	{
+		std::vector<float> values(4);
+		unsigned int i=0;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			values[i] = std::atof((*iter).c_str());
+			++i;
+		}
+
+		if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
+			values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
+			values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
+			values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
+		{
+			return computeRoi(image, values);
+		}
+		else
+		{
+			UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
+		}
+	}
+	return cv::Rect();
+}
+
+cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
 	if(!image.empty() && roiRatios.size() == 4)
 	{
@@ -222,6 +252,40 @@ cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 /////////////////////
 // Feature2D
 /////////////////////
+Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
+{
+	Feature2D * feature2D = 0;
+	switch(type)
+	{
+	case Feature2D::kFeatureSift:
+		feature2D = new SIFT(parameters);
+		break;
+	case Feature2D::kFeatureFastBrief:
+		feature2D = new FAST_BRIEF(parameters);
+		break;
+	case Feature2D::kFeatureFastFreak:
+		feature2D = new FAST_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureOrb:
+		feature2D = new ORB(parameters);
+		break;
+	case Feature2D::kFeatureGfttFreak:
+		feature2D = new GFTT_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureGfttBrief:
+		feature2D = new GFTT_BRIEF(parameters);
+		break;
+	case Feature2D::kFeatureBrisk:
+		feature2D = new BRISK(parameters);
+		break;
+	case Feature2D::kFeatureSurf:
+	default:
+		feature2D = new SURF(parameters);
+		type = Feature2D::kFeatureSurf;
+		break;
+	}
+	return feature2D;
+}
 std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
@@ -261,7 +325,10 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, in

 cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-	return generateDescriptorsImpl(image, keypoints);
+	cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
+	UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
+	UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+	return descriptors;
 }

 //////////////////////////
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -196,6 +196,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			{
 				_lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0);
 			}
+			else if(_workingMem.size()>0)
+			{
+				_lastSignature = uValue(_signatures, *_workingMem.rbegin(), (Signature*)0);
+			}

 			// Last id
 			_dbDriver->getLastNodeId(_idCount);
@@ -429,42 +433,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			_feature2D = 0;
 			_featureType = Feature2D::kFeatureUndef;
 		}
-		switch(detectorStrategy)
-		{
-		case Feature2D::kFeatureSift:
-			_feature2D = new SIFT(parameters);
-			_featureType = Feature2D::kFeatureSift;
-			break;
-		case Feature2D::kFeatureFastBrief:
-			_feature2D = new FAST_BRIEF(parameters);
-			_featureType = Feature2D::kFeatureFastBrief;
-			break;
-		case Feature2D::kFeatureFastFreak:
-			_feature2D = new FAST_FREAK(parameters);
-			_featureType = Feature2D::kFeatureFastFreak;
-			break;
-		case Feature2D::kFeatureOrb:
-			_feature2D = new ORB(parameters);
-			_featureType = Feature2D::kFeatureOrb;
-			break;
-		case Feature2D::kFeatureGfttFreak:
-			_feature2D = new GFTT_FREAK(parameters);
-			_featureType = Feature2D::kFeatureGfttFreak;
-			break;
-		case Feature2D::kFeatureGfttBrief:
-			_feature2D = new GFTT_BRIEF(parameters);
-			_featureType = Feature2D::kFeatureGfttBrief;
-			break;
-		case Feature2D::kFeatureBrisk:
-			_feature2D = new BRISK(parameters);
-			_featureType = Feature2D::kFeatureBrisk;
-			break;
-		case Feature2D::kFeatureSurf:
-		default:
-			_feature2D = new SURF(parameters);
-			_featureType = Feature2D::kFeatureSurf;
-			break;
-		}
+
+		_feature2D = Feature2D::create(detectorStrategy, parameters);
+		_featureType = detectorStrategy;
 	}
 	else if(_feature2D)
 	{
@@ -1605,6 +1576,10 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 			{
 				_lastSignature = this->_getSignature(*_stMem.rbegin());
 			}
+			else if(_workingMem.size())
+			{
+				_lastSignature = this->_getSignature(*_workingMem.rbegin());
+			}
 		}

 		if(	saveToDatabase &&
@@ -1727,12 +1702,15 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 			UDEBUG("Correspondences = %d", (int)inliersOld->size());

 			int inliersCount = 0;
+			std::vector<int> inliersV;
 			Transform t = util3d::transformFromXYZCorrespondences(
 					inliersOld,
 					inliersNew,
 					_bowInlierDistance,
 					_bowIterations,
-					&inliersCount);
+					true, 3.0, 10,
+					&inliersV);
+			inliersCount = inliersV.size();
 			if(!t.isNull() && inliersCount >= _bowMinInliers)
 			{
 				transform = t;
@@ -3038,16 +3016,12 @@ void Memory::extractKeypointsAndDescriptors(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors) const
 {
-	extractKeypointsAndDescriptors(image, cv::Mat(), 0,0,0,0, keypoints, descriptors);
+	extractKeypointsAndDescriptors(image, cv::Mat(), keypoints, descriptors);
 }

 void Memory::extractKeypointsAndDescriptors(
 		const cv::Mat & image,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors) const
 {
@@ -3056,12 +3030,12 @@ void Memory::extractKeypointsAndDescriptors(
 		UTimer timer;
 		if(_feature2D)
 		{
-			cv::Rect roi = computeRoi(image, _roiRatios);
+			cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
 			keypoints = _feature2D->generateKeypoints(image, 0, roi);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());

-			filterKeypointsByDepth(keypoints, depth, fx, fy, cx, cy, _wordsMaxDepth);
-			limitKeypoints(keypoints, _wordsPerImageTarget);
+			Feature2D::filterKeypointsByDepth(keypoints, depth, _wordsMaxDepth);
+			Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
 		}
 		else
 		{
@@ -3100,6 +3074,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
 	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
+	UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1);
 	UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);

 	PreUpdateThread preUpdateThread(_vwd);
@@ -3167,8 +3142,6 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)

 		this->extractKeypointsAndDescriptors(imageMono,
 				data.depth(),
-				data.depthFx(), data.depthFy(),
-				data.depthCx(), data.depthCy(),
 				keypoints,
 				descriptors);

@@ -3183,12 +3156,10 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		keypoints = data.keypoints();
 		descriptors = data.descriptors().clone();

-		filterKeypointsByDepth(keypoints, descriptors,
+		Feature2D::filterKeypointsByDepth(keypoints, descriptors,
 				data.depth(),
-				data.depthFx(), data.depthFy(),
-				data.depthCx(), data.depthCy(),
 				_wordsMaxDepth);
-		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+		Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}

 	if(_parallelized)
@@ -3229,9 +3200,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)

 	//3d words
 	std::multimap<int, pcl::PointXYZ> words3;
-	if(!data.depth().empty() && data.depthFx() && data.depthFy())
+	if(!data.depth().empty() && data.fx() && data.fy())
 	{
-		words3 = util3d::generateWords3(words, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), data.localTransform());
+		words3 = util3d::generateWords3(words, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
 	}

 	Signature * s;
@@ -3243,9 +3214,17 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		{
 			UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
 		}
-		cv::Mat depthMM = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
+		cv::Mat depthOrRightImage;
+		if(!data.depth().empty())
+		{
+			depthOrRightImage = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
+		}
+		else if(!data.rightImage().empty())
+		{
+			depthOrRightImage = data.rightImage();
+		}
 		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(depthMM, std::string(".png"));
+		util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
 		ctImage.start();
 		ctDepth.start();
 		ctImage.join();
@@ -3261,10 +3240,10 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			util3d::compressData(data.depth2d()),
 			imageBytes,
 			depthBytes,
-			data.depthFx(),
-			data.depthFy(),
-			data.depthCx(),
-			data.depthCy(),
+			data.fx(),
+			data.fy()>0.0f?data.fy():data.baseline(),
+			data.cx(),
+			data.cy(),
 			data.localTransform());
 		s->setImageRaw(data.image());
 		s->setDepthRaw(data.depth());
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -40,9 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/VWDictionary.h>
 #include "rtabmap/core/Signature.h"
+#include "rtabmap/core/Features2d.h"

 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/distances.h>

 #include <opencv2/gpu/gpu.hpp>

@@ -55,16 +57,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {

 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
-		_maxFeatures(Parameters::defaultOdomMaxWords()),
+		_maxFeatures(Parameters::defaultOdomMaxFeatures()),
+		_roiRatios(Parameters::defaultOdomRoiRatios()),
 		_minInliers(Parameters::defaultOdomMinInliers()),
 		_inlierDistance(Parameters::defaultOdomInlierDistance()),
 		_iterations(Parameters::defaultOdomIterations()),
-		_wordsRatio(Parameters::defaultOdomWordsRatio()),
+		_refineIterations(Parameters::defaultOdomRefineIterations()),
+		_featuresRatio(Parameters::defaultOdomFeaturesRatio()),
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
-		_localHistoryMaxSize(Parameters::defaultOdomLocalHistory()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0)
 {
@@ -74,10 +77,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
 	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
-	Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
+	Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
+	Parameters::parse(parameters, Parameters::kOdomFeaturesRatio(), _featuresRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
-	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
-	Parameters::parse(parameters, Parameters::kOdomLocalHistory(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
+	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 }

 void Odometry::reset()
@@ -126,23 +130,27 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
 //OdometryBOW
 OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	Odometry(parameters),
+	_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
 	_memory(0)
 {
+	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+
 	ParametersMap customParameters;
 	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
 	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
 	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
-	int nn = Parameters::defaultOdomNearestNeighbor();
-	float nndr = Parameters::defaultOdomNNDR();
-	int odomType = Parameters::defaultOdomType();
-	Parameters::parse(parameters, Parameters::kOdomNearestNeighbor(), nn);
-	Parameters::parse(parameters, Parameters::kOdomNNDR(), nndr);
-	Parameters::parse(parameters, Parameters::kOdomType(), odomType);
+	int nn = Parameters::defaultOdomBowNNType();
+	float nndr = Parameters::defaultOdomBowNNDR();
+	int featureType = Parameters::defaultOdomFeatureType();
+	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
 	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
 	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
-	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(odomType)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));

 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
@@ -204,10 +212,10 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 		{
 			Transform transform;
 			std::set<int> uniqueCorrespondences;
-			if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
+			if(newSignature->getWords3().size() < (unsigned int)(this->getFeaturesRatio() * float(previousSignature->getWords3().size())))
 			{
 				UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-						getWordsRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
+						this->getFeaturesRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
 			}
 			else if(!localMap_.empty() && !newSignature->getWords3().empty())
 			{
@@ -232,13 +240,16 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 					correspondences = inliers1->size();

 					// the transform returned is global odometry pose, not incremental one
+					std::vector<int> inliersV;
 					transform = util3d::transformFromXYZCorrespondences(
 							inliers2,
 							inliers1,
 							this->getInlierDistance(),
 							this->getIterations(),
-							&inliers);
+							this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+							&inliersV);

+					inliers = inliersV.size();
 					if(!transform.isNull())
 					{
 						// make it incremental
@@ -308,7 +319,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 				else
 				{
 					// remove words if history max size is reached
-					while(localMap_.size() && (int)localMap_.size() > this->getLocalHistoryMaxSize() && _memory->getStMem().size()>1)
+					while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
 					{
 						int nodeId = *_memory->getStMem().begin();
 						std::list<int> removedPts;
@@ -319,10 +330,10 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 						}
 					}

-					if(this->getLocalHistoryMaxSize() == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
+					if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
 					{
 						UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
-								(int)localMap_.size(), this->getLocalHistoryMaxSize(), (int)newSignature->getWords3().size());
+								(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
 					}

 					// update local map
@@ -402,6 +413,612 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 	return output;
 }

+//OdometryOpticalFlow
+OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
+	Odometry(parameters),
+	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+	flowIterations_(Parameters::defaultOdomFlowIterations()),
+	flowEps_(Parameters::defaultOdomFlowEps()),
+	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	subPixWinSize_(Parameters::defaultOdomFlowSubPixWinSize()),
+	subPixIterations_(Parameters::defaultOdomFlowSubPixIterations()),
+	subPixEps_(Parameters::defaultOdomFlowSubPixEps()),
+	lastCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+{
+	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
+
+	ParametersMap::const_iterator iter;
+	Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
+	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+	{
+		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
+	}
+	feature2D_ = Feature2D::create(detectorStrategy, parameters);
+}
+
+OdometryOpticalFlow::~OdometryOpticalFlow()
+{
+	delete feature2D_;
+}
+
+
+void OdometryOpticalFlow::reset()
+{
+	Odometry::reset();
+	lastFrame_ = cv::Mat();
+	lastCorners_.clear();
+	lastCorners3D_->clear();
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpticalFlow::computeTransform(
+		const SensorData & data,
+		int * quality,
+		int * features,
+		int * localMapSize)
+{
+	UDEBUG("");
+
+	if(!data.rightImage().empty())
+	{
+		//stereo
+		return computeTransformStereo(data, quality, features);
+	}
+	else
+	{
+		//rgbd
+		return computeTransformRGBD(data, quality, features);
+	}
+}
+
+Transform OdometryOpticalFlow::computeTransformStereo(
+		const SensorData & data,
+		int * quality,
+		int * features)
+{
+	UTimer timer;
+	Transform output;
+
+	int inliers = 0;
+	int correspondences = 0;
+	imgMatches_ = cv::Mat();
+
+	cv::Mat newLeftFrame;
+	// convert to grayscale
+	if(data.image().channels() > 1)
+	{
+		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newLeftFrame = data.image().clone();
+	}
+	cv::Mat newRightFrame = data.rightImage().clone();
+
+	std::vector<cv::Point2f> newCorners;
+	UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)lastCorners_.size(), lastFrame_.empty()?0:1, lastRightFrame_.empty()?0:1);
+	if(!lastFrame_.empty() && !lastRightFrame_.empty() && lastCorners_.size())
+	{
+		UDEBUG("");
+		// Find features in the new left image
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		cv::calcOpticalFlowPyrLK(
+				lastFrame_,
+				newLeftFrame,
+				lastCorners_,
+				newCorners,
+				status,
+				err,
+				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+		UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+		std::vector<cv::Point2f> lastCornersKept(status.size());
+		std::vector<cv::Point2f> newCornersKept(status.size());
+		int ki = 0;
+		for(unsigned int i=0; i<status.size(); ++i)
+		{
+			if(status[i])
+			{
+				lastCornersKept[ki] = lastCorners_[i];
+				newCornersKept[ki] = newCorners[i];
+				cv::Point2f pt = lastCorners_[i] - newCorners[i];
+				++ki;
+			}
+		}
+		lastCornersKept.resize(ki);
+		newCornersKept.resize(ki);
+
+		if(ki && ki >= this->getMinInliers())
+		{
+
+			std::vector<unsigned char> statusLast;
+			std::vector<float> errLast;
+			std::vector<cv::Point2f> lastCornersKeptRight;
+			cv::calcOpticalFlowPyrLK(
+						lastFrame_,
+						lastRightFrame_,
+						lastCornersKept,
+						lastCornersKeptRight,
+						statusLast,
+						errLast,
+						cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+
+			UDEBUG("");
+			std::vector<cv::KeyPoint> lastKpts, newKpts;
+			/*cv::KeyPoint::convert(lastCornersKept, lastKpts);
+			cv::KeyPoint::convert(newCornersKept, newKpts);
+			std::vector<cv::DMatch> good_matches(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
+						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+			UDEBUG("");*/
+
+			std::vector<unsigned char> statusNew;
+			std::vector<float> errNew;
+			std::vector<cv::Point2f> newCornersKeptRight;
+			cv::calcOpticalFlowPyrLK(
+						newLeftFrame,
+						newRightFrame,
+						newCornersKept,
+						newCornersKeptRight,
+						statusNew,
+						errNew,
+						cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+
+			UDEBUG("Getting correspondences begin");
+			// Get 3D correspondences
+			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
+			correspondencesLast->resize(statusLast.size());
+			correspondencesNew->resize(statusLast.size());
+			int oi = 0;
+			lastKpts.resize(statusLast.size());
+			newKpts.resize(statusLast.size());
+			for(unsigned int i=0; i<statusLast.size(); ++i)
+			{
+				if(statusLast[i] && statusNew[i])
+				{
+					float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
+					float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
+					if(lastDisparity > 0.0f && newDisparity > 0.0f)
+					{
+						pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3d(
+								lastCornersKept[i],
+								lastDisparity,
+								data.cx(), data.cy(), data.fx(), data.baseline());
+						pcl::PointXYZ newPt3D = util3d::projectDisparityTo3d(
+								newCornersKept[i],
+								newDisparity,
+								data.cx(), data.cy(), data.fx(), data.baseline());
+
+						if(pcl::isFinite(lastPt3D) && uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth()) &&
+						   pcl::isFinite(newPt3D) && uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))
+						{
+							//Add 3D correspondences!
+							lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
+							newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
+							correspondencesLast->at(oi) = lastPt3D;
+							correspondencesNew->at(oi) = newPt3D;
+							lastKpts[oi].pt = lastCornersKept[i];
+							newKpts[oi].pt = newCornersKept[i];
+							++oi;
+						}
+					}
+				}
+			}// end loop
+			correspondencesLast->resize(oi);
+			correspondencesNew->resize(oi);
+			lastKpts.resize(oi);
+			newKpts.resize(oi);
+			correspondences = oi;
+			lastCorners3D_ = correspondencesNew;
+			UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
+
+			/*good_matches.resize(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::Mat imgInliers;
+			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
+						   good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+			imgMatches_.push_back(imgInliers);
+			UDEBUG("");*/
+
+			if(correspondences >= this->getMinInliers())
+			{
+				std::vector<int> inliersV;
+				UTimer timerRANSAC;
+				output = util3d::transformFromXYZCorrespondences(
+						correspondencesNew,
+						correspondencesLast,
+						this->getInlierDistance(),
+						this->getIterations(),
+						this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+						&inliersV);
+				UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
+
+				inliers = (int)inliersV.size();
+				if(quality)
+				{
+					*quality = inliers;
+				}
+
+				if(inliers < this->getMinInliers())
+				{
+					output.setNull();
+					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+				}
+
+				/*if(correspondencesLast->size() >= 6)
+				{
+					UWARN("saved pcd");
+					pcl::io::savePCDFile("last.pcd", *correspondencesLast);
+					pcl::io::savePCDFile("new.pcd", *correspondencesNew);
+					correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
+					pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
+				}*/
+			}
+			else
+			{
+				UWARN("Not enough correspondences (%d)", correspondences);
+			}
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+	}
+
+	newCorners.clear();
+	if(!output.isNull())
+	{
+		// Update frame, reset saved last transform
+		savedLastRefFrameTransform_.setNull();
+
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			newCorners.resize(data.keypoints().size());
+			for(unsigned int i=0; i<data.keypoints().size(); ++i)
+			{
+				newCorners[i] = data.keypoints().at(i).pt;
+			}
+		}
+		else
+		{
+			// generate kpts
+			std::vector<cv::KeyPoint> newKtps;
+			cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newLeftFrame, this->getMaxFeatures(), roi);
+			Feature2D::limitKeypoints(newKtps, this->getMaxFeatures());
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					UDEBUG("cv::cornerSubPix() begin");
+					cv::cornerSubPix(newLeftFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+					UDEBUG("cv::cornerSubPix() end");
+				}
+			}
+		}
+
+		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		{
+			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
+					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+		}
+		else if((int)newCorners.size() > this->getMinInliers())
+		{
+			lastFrame_ = newLeftFrame;
+			lastRightFrame_ = newRightFrame;
+			lastCorners_ = newCorners;
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+		}
+	}
+	else if(!output.isNull())
+	{
+		output.setNull();
+	}
+
+	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			!output.isNull()?"true":"false");
+
+	return output;
+}
+
+Transform OdometryOpticalFlow::computeTransformRGBD(
+		const SensorData & data,
+		int * quality,
+		int * features)
+{
+	UTimer timer;
+	Transform output;
+
+	int inliers = 0;
+	int correspondences = 0;
+	imgMatches_ = cv::Mat();
+
+	cv::Mat newFrame;
+	// convert to grayscale
+	if(data.image().channels() > 1)
+	{
+		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newFrame = data.image().clone();
+	}
+
+	float updatePixels = 0.0f;
+	bool updateFrame = false;
+
+	std::vector<cv::Point2f> newCorners;
+	if(!lastFrame_.empty() && lastCorners_.size() && lastCorners3D_->size())
+	{
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		cv::calcOpticalFlowPyrLK(
+				lastFrame_,
+				newFrame,
+				lastCorners_,
+				newCorners,
+				status,
+				err,
+				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+		UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
+		correspondencesLast->resize(lastCorners_.size());
+		correspondencesNew->resize(lastCorners_.size());
+		int oi=0;
+
+		std::vector<cv::KeyPoint> lastKpts(lastCorners_.size());
+		std::vector<cv::KeyPoint> newKpts(lastCorners_.size());
+
+		UASSERT(lastCorners_.size() == lastCorners3D_->size());
+		UDEBUG("lastCorners3D_ = %d", lastCorners3D_->size());
+		float sumSqrdDistance = 0.0f;
+		int flowInliers = 0;
+		for(unsigned int i=0; i<status.size(); ++i)
+		{
+			if(status[i] && pcl::isFinite(lastCorners3D_->at(i)) &&
+				uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
+				uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
+			{
+				pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
+						data.cx(), data.cy(), data.fx(), data.fy(), true);
+				if(pcl::isFinite(pt) &&
+					uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
+					uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
+					uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))
+				{
+					pt = util3d::transformPoint(pt, data.localTransform());
+					correspondencesLast->at(oi) = lastCorners3D_->at(i);
+					correspondencesNew->at(oi) = pt;
+
+					cv::Point2f diff = newCorners[i]-lastCorners_[i];
+					sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
+
+					lastKpts[oi].pt = lastCorners_[i];
+					newKpts[oi].pt = newCorners[i];
+
+					++oi;
+				}
+				++flowInliers;
+			}
+			else if(status[i])
+			{
+				++flowInliers;
+			}
+		}
+		UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
+		float meanPixel = -1;
+		if(oi)
+		{
+			float meanPixel = sumSqrdDistance/(float)oi;
+			if(meanPixel >= updatePixels*updatePixels)
+			{
+				updateFrame = true;
+			}
+		}
+		UDEBUG("mean pixel distance = %f", meanPixel);
+
+		lastKpts.resize(oi);
+		newKpts.resize(oi);
+		correspondencesLast->resize(oi);
+		correspondencesNew->resize(oi);
+		correspondences = oi;
+		if(correspondences >= this->getMinInliers())
+		{
+			std::vector<int> inliersV;
+			UTimer timerRANSAC;
+			output = util3d::transformFromXYZCorrespondences(
+					correspondencesNew,
+					correspondencesLast,
+					this->getInlierDistance(),
+					this->getIterations(),
+					this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+					&inliersV);
+			UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
+
+			inliers = (int)inliersV.size();
+			if(quality)
+			{
+				*quality = inliers;
+			}
+
+			if(inliers < this->getMinInliers())
+			{
+				output.setNull();
+				UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+			}
+
+			/*std::vector<cv::DMatch> good_matches(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::drawMatches( lastFrame_, lastKpts, newFrame, newKpts,
+						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );*/
+		}
+		else
+		{
+			UWARN("Not enough correspondences (%d)", correspondences);
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+		updateFrame = true;
+	}
+
+	newCorners.clear();
+	if(!output.isNull() && updateFrame)
+	{
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			newCorners.resize(data.keypoints().size());
+			for(unsigned int i=0; i<data.keypoints().size(); ++i)
+			{
+				newCorners[i] = data.keypoints().at(i).pt;
+			}
+		}
+		else
+		{
+			// generate kpts
+			std::vector<cv::KeyPoint> newKtps;
+			cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
+			Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
+			Feature2D::limitKeypoints(newKtps, this->getMaxFeatures());
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					cv::cornerSubPix(newFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+				}
+			}
+		}
+
+		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		{
+			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
+					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+		}
+		else if((int)newCorners.size() > this->getMinInliers())
+		{
+			// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
+			newCorners3D->resize(newCorners.size());
+			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
+			int oi=0;
+			for(unsigned int i=0; i<newCorners.size(); ++i)
+			{
+				if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)-1.0f) &&
+				   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)-1.0f))
+				{
+					pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
+							data.cx(), data.cy(), data.fx(), data.fy(), true);
+					if(pcl::isFinite(pt) &&
+						uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
+						uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
+						uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))
+					{
+						pt = util3d::transformPoint(pt, data.localTransform());
+						newCorners3D->at(oi) = pt;
+						newCornersFiltered[oi] = newCorners[i];
+						++oi;
+					}
+				}
+			}
+			newCornersFiltered.resize(oi);
+			newCorners3D->resize(oi);
+			if((int)newCornersFiltered.size() > this->getMinInliers())
+			{
+				lastFrame_ = newFrame;
+				lastCorners_ = newCornersFiltered;
+				lastCorners3D_ = newCorners3D;
+			}
+			else
+			{
+				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
+						(int)newCornersFiltered.size(), (int)lastCorners3D_->size(), this->getMinInliers());
+			}
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+		}
+	}
+	else if(!output.isNull())
+	{
+		output = Transform::getIdentity();
+	}
+
+	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			updateFrame||output.isNull()?"true":"false");
+	return output;
+}
+
 // OdometryICP
 OdometryICP::OdometryICP(int decimation,
 		float voxelSize,
@@ -444,10 +1061,10 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 						data.depth(),
-						data.depthFx(),
-						data.depthFy(),
-						data.depthCx(),
-						data.depthCy(),
+						data.fx(),
+						data.fy(),
+						data.cx(),
+						data.cy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
@@ -618,7 +1235,7 @@ void OdometryThread::mainLoop()

 void OdometryThread::addData(const SensorData & data)
 {
-	if(data.image().empty() || data.depth().empty() || data.depthFx() == 0.0f || data.depthFy() == 0.0f)
+	if(data.image().empty() || data.depth().empty() || data.fx() == 0.0f || data.fy() == 0.0f)
 	{
 		ULOGGER_ERROR("image empty !?");
 		return;
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -1581,7 +1581,6 @@ bool Rtabmap::process(const SensorData & data)

 			if(_publishImage)
 			{
-				std::map<int, int> mapIds;
 				std::map<int, std::vector<unsigned char> > images;
 				std::map<int, std::vector<unsigned char> > depths;
 				std::map<int, std::vector<unsigned char> > depth2ds;
@@ -1629,7 +1628,6 @@ bool Rtabmap::process(const SensorData & data)
 						im = _memory->getImage(ids[i]);
 					}
 					UASSERT(_memory->getSignature(ids[i]) != 0);
-					mapIds.insert(std::make_pair(ids[i], _memory->getSignature(ids[i])->mapId()));
 					if(!im.empty())
 					{
 						images.insert(std::make_pair(ids[i], im));
@@ -1641,7 +1639,6 @@ bool Rtabmap::process(const SensorData & data)
 					UWARN("getting data[%d] time = %fs", (int)ids.size(), tmpTimer.ticks());
 				}

-				statistics_.setMapIds(mapIds);
 				statistics_.setImages(images);
 				statistics_.setDepths(depths);
 				statistics_.setDepth2ds(depth2ds);
@@ -1756,6 +1753,13 @@ bool Rtabmap::process(const SensorData & data)
 		//Poses, place this after Transfer! (_optimizedPoses may change)
 		if(_rgbdSlamMode)
 		{
+			std::map<int, int> mapIds;
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			}
+
+			statistics_.setMapIds(mapIds);
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapCorrection(_mapCorrection);
@@ -2345,6 +2349,10 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 		}
 	}
+	else if(_memory->getStMem().size() || _memory->getWorkingMem().size())
+	{
+		UERROR("Last working signature is null!?");
+	}
 }

 void Rtabmap::getGraph(
@@ -2378,6 +2386,10 @@ void Rtabmap::getGraph(
 			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 		}
 	}
+	else if(_memory->getStMem().size() || _memory->getWorkingMem().size())
+	{
+		UERROR("Last working signature is null!?");
+	}
 }

 void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/utilite/ULogger.h"

 namespace rtabmap
 {
@@ -38,7 +39,7 @@ SensorData::SensorData() :
 	_image(cv::Mat()),
 	_id(0),
 	_fx(0.0f),
-	_fy(0.0f),
+	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity())
@@ -50,18 +51,20 @@ SensorData::SensorData(const cv::Mat & image,
 	_image(image),
 	_id(id),
 	_fx(0.0f),
-	_fy(0.0f),
+	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity())
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
 }

 	// Metric constructor
 SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -69,22 +72,29 @@ SensorData::SensorData(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_depth(depth),
+	_depthOrRightImage(depthOrRightImage),
 	_fx(fx),
-	_fy(fy),
+	_fyOrBaseline(fyOrBaseline),
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform)
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
+	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+			depthOrRightImage.type() == CV_8U);     // Right stereo image
+	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
+	UASSERT(!_localTransform.isNull());
 }

 	// Metric constructor + 2d depth
 SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  const cv::Mat & depth2d,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -92,15 +102,22 @@ SensorData::SensorData(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_depth(depth),
+	_depthOrRightImage(depthOrRightImage),
 	_depth2d(depth2d),
 	_fx(fx),
-	_fy(fy),
+	_fyOrBaseline(fyOrBaseline),
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform)
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
+	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+			depthOrRightImage.type() == CV_8U);     // Right stereo image
+	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
+	UASSERT(!_localTransform.isNull());
 }

 bool SensorData::empty() const
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -443,13 +443,18 @@ pcl::PointXYZ getDepth(
 		bool smoothing,
 		float maxZError)
 {
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+
 	pcl::PointXYZ pt;
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();

-	if(!(int(x) >=0 && int(x)<depthImage.cols && int(y) >=0 && int(y)<depthImage.rows))
+	int u = int(x+0.5f);
+	int v = int(y+0.5f);
+
+	if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
 	{
-		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f, y=%f, cols=%d, rows=%d)",
-				x,y,depthImage.cols, depthImage.rows);
+		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
+				x,u,y,v,depthImage.cols, depthImage.rows);
 		pt.x = pt.y = pt.z = bad_point;
 		return pt;
 	}
@@ -462,8 +467,6 @@ pcl::PointXYZ getDepth(
 	//  | 1 | 2 | 1 |
 	//  | 2 | 4 | 2 |
 	//  | 1 | 2 | 1 |
-	int u = int(x+0.5f);
-	int v = int(y+0.5f);
 	int u_start = std::max(u-1, 0);
 	int v_start = std::max(v-1, 0);
 	int u_end = std::min(u+1, depthImage.cols-1);
@@ -721,6 +724,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation)
 {
+	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
 	{
@@ -775,6 +779,129 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	return cloud;
 }

+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDisparity,
+		float cx, float cy,
+		float fx, float baseline,
+		int decimation)
+{
+	UASSERT(imageRgb.rows == imageDisparity.rows &&
+			imageRgb.cols == imageDisparity.cols &&
+			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	if(decimation < 1)
+	{
+		return cloud;
+	}
+
+	bool mono;
+	if(imageRgb.channels() == 3) // BGR
+	{
+		mono = false;
+	}
+	else if(imageRgb.channels() == 1) // Mono
+	{
+		mono = true;
+	}
+	else
+	{
+		return cloud;
+	}
+
+	//cloud.header = cameraInfo.header;
+	cloud->height = imageRgb.rows/decimation;
+	cloud->width  = imageRgb.cols/decimation;
+	cloud->is_dense = false;
+	cloud->resize(cloud->height * cloud->width);
+
+	for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
+	{
+		for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
+		{
+			pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
+			if(!mono)
+			{
+				pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
+				pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
+				pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
+			}
+			else
+			{
+				unsigned char v = imageRgb.at<unsigned char>(h,w);
+				pt.b = v;
+				pt.g = v;
+				pt.r = v;
+			}
+
+			float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
+			pcl::PointXYZ ptXYZ = projectDisparityTo3d(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+			pt.x = ptXYZ.x;
+			pt.y = ptXYZ.y;
+			pt.z = ptXYZ.z;
+		}
+	}
+	return cloud;
+}
+
+cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rightImage)
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+			leftImage.cols == rightImage.cols &&
+			leftImage.rows == rightImage.rows);
+	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
+	cv::Mat disparity;
+	stereo(leftImage, rightImage, disparity, CV_16S);
+	cv::filterSpeckles(disparity, 0, 1000, 16);
+	return disparity;
+}
+
+// inspired from ROS image_geometry/src/stereo_camera_model.cpp
+pcl::PointXYZ projectDisparityTo3d(
+		const cv::Point2f & pt,
+		float disparity,
+		float cx, float cy, float fx, float baseline)
+{
+	if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
+	{
+		float W = disparity/baseline;// + (right_.cx() - left_.cx()) / Tx;
+		return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
+	}
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+	return pcl::PointXYZ(bad_point, bad_point, bad_point);
+}
+
+cv::Mat depthFromDisparity(const cv::Mat & disparity,
+		float cx, float cy, float fx, float baseline,
+		int type)
+{
+	UASSERT(disparity.type() == CV_32FC1 || disparity.type() == CV_16S);
+	UASSERT(type == CV_32FC1 || type == CV_16U);
+	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
+	for (int i = 0; i < disparity.rows; i++)
+	{
+		for (int j = 0; j < disparity.cols; j++)
+		{
+			float disparity_value = disparity.type() == CV_16S?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
+			if (disparity_value > 0.0f)
+			{
+				// baseline * focal / disparity
+				float d = baseline * fx / disparity_value;
+				if(depth.type() == CV_32FC1)
+				{
+					depth.at<float>(i,j) = d;
+				}
+				else
+				{
+					depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
+				}
+			}
+		}
+	}
+	return depth;
+}
+
 cv::Mat depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
 {
 	cv::Mat depth2d(1, (int)cloud.size(), CV_32FC2);
@@ -1131,59 +1258,168 @@ Transform transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
 		double inlierThreshold,
 		int iterations,
-		int * inliers)
+		bool refineModel,
+		double refineModelSigma,
+		int refineModelIterations,
+		std::vector<int> * inliersOut)
 {
+	//NOTE: this method is a mix of two methods:
+	//  - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
+	//  - refineModel() in pcl/sample_consensus/sac.h
+
 	Transform transform;
-	if(cloud1->size() && cloud1->size() == cloud2->size())
+	if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
 	{
-		// Robust to outliers RANSAC
-		pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
-		for(unsigned int i = 0; i<cloud1->size(); ++i)
+		// RANSAC
+		UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
+		std::vector<int> source_indices (cloud2->size());
+		std::vector<int> target_indices (cloud1->size());
+
+		// Copy the query-match indices
+		for (size_t i = 0; i < cloud1->size(); ++i)
 		{
-			correspondences->push_back(pcl::Correspondence(i, i, pcl::euclideanDistance(cloud2->at(i), cloud1->at(i))));
+			source_indices[i] = i;
+			target_indices[i] = i;
 		}

-		pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> crsc;
-		crsc.setInputCorrespondences(correspondences);
-		crsc.setInputSource(cloud2);
-		crsc.setInputTarget(cloud1);
-		crsc.setMaximumIterations(iterations);
-		crsc.setInlierThreshold(inlierThreshold);
-		crsc.setRefineModel(true);
-		pcl::Correspondences correspondencesInliers;
-		crsc.getCorrespondences(correspondencesInliers);
-		UDEBUG("RANSAC inliers=%d outliers=%d", (int)correspondencesInliers.size(), (int)correspondences->size()-(int)correspondencesInliers.size());
-		transform = util3d::transformFromEigen4f(crsc.getBestTransformation());
+		// From the set of correspondences found, attempt to remove outliers
+		// Create the registration model
+		pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
+		model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
+		// Pass the target_indices
+		model->setInputTarget (cloud1, target_indices);
+		// Create a RANSAC model
+		pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
+		sac.setMaxIterations(iterations);

-		/*UDEBUG("RANSAC=%s", transform.prettyPrint().c_str());
-
-		pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
-		Eigen::Matrix4f transform_svd;
-		trans_est.estimateRigidTransformation (*cloud2, *cloud1, correspondencesInliers, transform_svd);
-		transform = util3d::transformFromEigen4f(transform_svd);
-		UDEBUG("SVD=%s", transform.prettyPrint().c_str());*/
-
-		if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
+		// Compute the set of inliers
+		if(sac.computeModel())
 		{
-			//Wrong transform
-			UDEBUG("Wrong transform: identity with full inliers");
-			transform.setNull();
-		}
+			std::vector<int> inliers;
+			Eigen::VectorXf model_coefficients;

-		if(inliers)
+			sac.getInliers(inliers);
+			sac.getModelCoefficients (model_coefficients);
+
+			if (refineModel)
+			{
+				double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
+				double error_threshold = inlierThreshold;
+				double sigma_sqr = refineModelSigma * refineModelSigma;
+				int refine_iterations = 0;
+				bool inlier_changed = false, oscillating = false;
+				std::vector<int> new_inliers, prev_inliers = inliers;
+				std::vector<size_t> inliers_sizes;
+				Eigen::VectorXf new_model_coefficients = model_coefficients;
+				do
+				{
+					// Optimize the model coefficients
+					model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
+					inliers_sizes.push_back (prev_inliers.size ());
+
+					// Select the new inliers based on the optimized coefficients and new threshold
+					model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
+					UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.",
+							prev_inliers.size (), new_inliers.size (), error_threshold);
+
+					if (new_inliers.empty ())
+					{
+						++refine_iterations;
+						if (refine_iterations >= refineModelIterations)
+						{
+							break;
+						}
+						continue;
+					}
+
+					// Estimate the variance and the new threshold
+					double variance = model->computeVariance ();
+					error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
+
+					UDEBUG ("RANSAC refineModel: New estimated error threshold: %g on iteration %d out of %d.",
+						  error_threshold, refine_iterations, refineModelIterations);
+					inlier_changed = false;
+					std::swap (prev_inliers, new_inliers);
+
+					// If the number of inliers changed, then we are still optimizing
+					if (new_inliers.size () != prev_inliers.size ())
+					{
+						// Check if the number of inliers is oscillating in between two values
+						if (inliers_sizes.size () >= 4)
+						{
+							if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
+							inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
+							{
+								oscillating = true;
+								break;
+							}
+						}
+						inlier_changed = true;
+						continue;
+					}
+
+					// Check the values of the inlier set
+					for (size_t i = 0; i < prev_inliers.size (); ++i)
+					{
+						// If the value of the inliers changed, then we are still optimizing
+						if (prev_inliers[i] != new_inliers[i])
+						{
+							inlier_changed = true;
+							break;
+						}
+					}
+				}
+				while (inlier_changed && ++refine_iterations < refineModelIterations);
+
+				// If the new set of inliers is empty, we didn't do a good job refining
+				if (new_inliers.empty ())
+				{
+					UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
+				}
+
+				if (oscillating)
+				{
+					UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
+				}
+
+				std::swap (inliers, new_inliers);
+				model_coefficients = new_model_coefficients;
+			}
+
+			if (inliers.size() >= 3)
+			{
+				if(inliersOut)
+				{
+					*inliersOut = inliers;
+				}
+
+				// get best transformation
+				Eigen::Matrix4f bestTransformation;
+				bestTransformation.row (0) = model_coefficients.segment<4>(0);
+				bestTransformation.row (1) = model_coefficients.segment<4>(4);
+				bestTransformation.row (2) = model_coefficients.segment<4>(8);
+				bestTransformation.row (3) = model_coefficients.segment<4>(12);
+
+				transform = util3d::transformFromEigen4f(bestTransformation);
+				UDEBUG("RANSAC inliers=%zu/%zu tf=%s", inliers.size(), cloud1->size(), transform.prettyPrint().c_str());
+
+				return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
+			}
+			else
+			{
+				UDEBUG("RANSAC: Model with inliers < 3");
+			}
+		}
+		else
 		{
-			*inliers = (int)correspondencesInliers.size();
+			UDEBUG("RANSAC: Failed to find model");
 		}
-
-		//std::cout << "transformMatrix: " << transformMatrix << std::endl;
-
-		//std::cout << "quality: " << float(correspondencesRej.size()) / float(correspondences->size());
 	}
 	else
 	{
-		UDEBUG("not enough points to compute the transform");
+		UDEBUG("Not enough points to compute the transform");
 	}
-	return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
+	return Transform();
 }

 // return transform from source to target (All points must be finite!!!)