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

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() {}
 

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;
 

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:

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");

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;

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,

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
 	{

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

corelib/src/Features2d.cpp

@@ -46,43 +46,41 @@ 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)
+			{
+				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);
 		keypoints = output;
 
@@ -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;
 }
 
 //////////////////////////

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());

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;

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)

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

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;
+
+			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;
 					}
 
-		if(inliers)
+					// 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 ())
 					{
-			*inliers = (int)correspondencesInliers.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;
 					}
 
-		//std::cout << "transformMatrix: " << transformMatrix << std::endl;
+					// 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);
 
-		//std::cout << "quality: " << float(correspondencesRej.size()) / float(correspondences->size());
+				// 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("not enough points to compute the transform");
+				UDEBUG("RANSAC: Model with inliers < 3");
 			}
-	return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
+		}
+		else
+		{
+			UDEBUG("RANSAC: Failed to find model");
+		}
+	}
+	else
+	{
+		UDEBUG("Not enough points to compute the transform");
+	}
+	return Transform();
 }
 
 // return transform from source to target (All points must be finite!!!)

examples/RGBDMapping/MapBuilder.h

@@ -96,16 +96,16 @@ private slots:
 			if(data.depth().cols == data.image().cols &&
 			   data.depth().rows == data.image().rows &&
 			   !data.depth().empty() &&
-			   data.depthFx() > 0.0f &&
-			   data.depthFy() > 0.0f)
+			   data.fx() > 0.0f &&
+			   data.fy() > 0.0f)
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 					data.image(),
 					data.depth(),
-					data.depthCx(),
-					data.depthCy(),
-					data.depthFx(),
-					data.depthFy(),
+					data.cx(),
+					data.cy(),
+					data.fx(),
+					data.fy(),
 					2); // decimation // high definition
 				if(cloud->size())
 				{

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -189,7 +189,7 @@ public:
 	bool isStatisticsPublished() const;
 	double getLoopThr() const;
 	double getVpThr() const;
-	double getExpThr() const;
+	int getOdomStrategy() const;
 
 	//
 	void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}

guilib/src/CloudViewer.cpp

@@ -74,7 +74,6 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
-		_lastPose(Transform::getIdentity()),
 		_workingDirectory(".")
 {
 	this->setMinimumSize(200, 200);
@@ -438,6 +437,7 @@ void CloudViewer::clearTrajectory()
 {
 	_trajectory->clear();
 	_visualizer->removeShape("trajectory");
+	_lastPose.setNull();
 	this->render();
 }
 
@@ -502,8 +502,13 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
 			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory");
 		}
 
-		if(pose != _lastPose)
+		if(pose != _lastPose || _lastPose.isNull())
 		{
+			if(_lastPose.isNull())
+			{
+				_lastPose.setIdentity();
+			}
+
 			std::vector<pcl::visualization::Camera> cameras;
 			_visualizer->getCameras(cameras);
 

guilib/src/DatabaseViewer.cpp

@@ -139,7 +139,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 
 	ui_->constraintsViewer->setCameraLockZ(false);
-
+	ui_->constraintsViewer->updateCameraPosition(Transform::getIdentity());
 }
 
 DatabaseViewer::~DatabaseViewer()
@@ -470,10 +470,12 @@ void DatabaseViewer::updateIds()
 		ui_->horizontalSlider_neighbors->setEnabled(false);
 	}
 
+	if(ids_.size())
+	{
 		updateLoopClosuresSlider();
-
 		updateGraphView();
 	}
+}
 
 void DatabaseViewer::generateGraph()
 {
@@ -769,12 +771,35 @@ void DatabaseViewer::view3DMap()
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 							cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+							UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
+							UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
+							if(depthMat.type() == CV_8UC1)
+							{
+								cv::Mat leftImg;
+								if(imageMat.channels() == 3)
+								{
+									cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
+								}
+								else
+								{
+									leftImg = imageMat;
+								}
+								cloud = rtabmap::util3d::cloudFromDisparityRGB(
+									imageMat,
+									util3d::disparityFromStereoImages(leftImg, depthMat),
+									cx, cy,
+									fx, fy,
+									decimation);
+							}
+							else
+							{
 								cloud = rtabmap::util3d::cloudFromDepthRGB(
 										imageMat,
 										depthMat,
 										cx, cy,
 										fx, fy,
 										decimation);
+							}
 
 							if(maxDepth)
 							{
@@ -872,12 +897,35 @@ void DatabaseViewer::generate3DMap()
 									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 									cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
 									cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+									UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
+									UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
+									if(depthMat.type() == CV_8UC1)
+									{
+										cv::Mat leftImg;
+										if(imageMat.channels() == 3)
+										{
+											cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
+										}
+										else
+										{
+											leftImg = imageMat;
+										}
+										cloud = rtabmap::util3d::cloudFromDisparityRGB(
+											imageMat,
+											util3d::disparityFromStereoImages(leftImg, depthMat),
+											cx, cy,
+											fx, fy,
+											decimation);
+									}
+									else
+									{
 										cloud = rtabmap::util3d::cloudFromDepthRGB(
 												imageMat,
 												depthMat,
 												cx, cy,
 												fx, fy,
 												decimation);
+									}
 
 									if(maxDepth)
 									{
@@ -1316,6 +1364,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 	cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
 	cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
 	cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);
+	UASSERT(imageA.empty() || imageA.type()==CV_8UC3 || imageA.type() == CV_8UC1);
+	UASSERT(depthA.empty() || depthA.type()==CV_8UC1 || depthA.type() == CV_16UC1 || depthA.type() == CV_32FC1);
 
 	std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
 	memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
@@ -1325,23 +1375,66 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
 
 	//cloud 3d
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+	if(depthA.type() == CV_8UC1)
+	{
+		cv::Mat leftImg;
+		if(imageA.channels() == 3)
+		{
+			cv::cvtColor(imageA, leftImg, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftImg = imageA;
+		}
+		cv::Mat disparity = util3d::disparityFromStereoImages(leftImg, depthA);
+		cloudA = rtabmap::util3d::cloudFromDisparityRGB(
+				imageA,
+				disparity,
+				cxA, cyA,
+				fxA, fyA,
+				1);
+	}
+	else
+	{
 		cloudA = rtabmap::util3d::cloudFromDepthRGB(
 				imageA,
 				depthA,
 				cxA, cyA,
 				fxA, fyA,
 				1);
+	}
 
 	cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
 	cloudA = rtabmap::util3d::transformPointCloud(cloudA, localTransformA);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+	if(depthB.type() == CV_8UC1)
+	{
+		cv::Mat leftImg;
+		if(imageB.channels() == 3)
+		{
+			cv::cvtColor(imageB, leftImg, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftImg = imageB;
+		}
+		cloudB = rtabmap::util3d::cloudFromDisparityRGB(
+				imageB,
+				util3d::disparityFromStereoImages(leftImg, depthB),
+				cxB, cyB,
+				fxB, fyB,
+				1);
+	}
+	else
+	{
 		cloudB = rtabmap::util3d::cloudFromDepthRGB(
 				imageB,
 				depthB,
 				cxB, cyB,
 				fxB, fyB,
 				1);
+	}
 
 	cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
 	cloudB = rtabmap::util3d::transformPointCloud(cloudB, t*localTransformB);

guilib/src/MainWindow.cpp

@@ -603,18 +603,18 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 		if(data.depth().cols == data.image().cols &&
 		   data.depth().rows == data.image().rows &&
 		   !data.depth().empty() &&
-		   data.depthFx() > 0.0f &&
-		   data.depthFy() > 0.0f &&
+		   data.fx() > 0.0f &&
+		   data.fy() > 0.0f &&
 		   _preferencesDialog->isCloudsShown(1))
 		{
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 			cloud = createCloud(0,
 					data.image(),
 					data.depth(),
-					data.depthFx(),
-					data.depthFy(),
-					data.depthCx(),
-					data.depthCy(),
+					data.fx(),
+					data.fy(),
+					data.cx(),
+					data.cy(),
 					data.localTransform(),
 					pose,
 					_preferencesDialog->getCloudVoxelSize(1),
@@ -2084,7 +2084,15 @@ void MainWindow::startDetection()
 				UERROR("OdomThread must be already deleted here?!");
 				delete _odomThread;
 			}
-			Odometry * odom = new OdometryBOW(parameters);
+			Odometry * odom;
+			if(_preferencesDialog->getOdomStrategy() == 1)
+			{
+				odom = new OdometryOpticalFlow(parameters);
+			}
+			else
+			{
+				odom = new OdometryBOW(parameters);
+			}
 			_odomThread = new OdometryThread(odom);
 
 			UEventsManager::addHandler(_odomThread);
@@ -2179,7 +2187,15 @@ void MainWindow::startDetection()
 				UERROR("OdomThread must be already deleted here?!");
 				delete _odomThread;
 			}
-			Odometry * odom = new OdometryBOW(parameters);
+			Odometry * odom;
+			if(_preferencesDialog->getOdomStrategy() == 1)
+			{
+				odom = new OdometryOpticalFlow(parameters);
+			}
+			else
+			{
+				odom = new OdometryBOW(parameters);
+			}
 			_odomThread = new OdometryThread(odom);
 
 			UEventsManager::addHandler(_odomThread);

guilib/src/OdometryViewer.cpp

@@ -91,7 +91,7 @@ void OdometryViewer::processData()
 	}
 	dataMutex_.unlock();
 
-	if(!data.image().empty() && !data.depth().empty() && data.depthFx()>0.0f && data.depthFy()>0.0f && this->isVisible())
+	if(!data.image().empty() && !data.depth().empty() && data.fx()>0.0f && data.fy()>0.0f && this->isVisible())
 	{
 		UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
 
@@ -101,8 +101,8 @@ void OdometryViewer::processData()
 		cloud = util3d::cloudFromDepthRGB(
 				data.image(),
 				data.depth(),
-				data.depthCx(), data.depthCy(),
-			    data.depthFx(), data.depthFy(),
+				data.cx(), data.cy(),
+			    data.fx(), data.fy(),
 			    decimation_);
 
 		if(voxelSize_ > 0.0f)

guilib/src/PreferencesDialog.cpp

@@ -453,19 +453,33 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
 
 	//Odometry
-	_ui->odom_type->setObjectName(Parameters::kOdomType().c_str());
+	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
+	_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
 	_ui->odom_linearUpdate->setObjectName(Parameters::kOdomLinearUpdate().c_str());
 	_ui->odom_angularUpdate->setObjectName(Parameters::kOdomAngularUpdate().c_str());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
-	_ui->odom_localHistory->setObjectName(Parameters::kOdomLocalHistory().c_str());
-	_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxWords().c_str());
+	_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
+	_ui->odom_ratio->setObjectName(Parameters::kOdomFeaturesRatio().c_str());
 	_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
 	_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
 	_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
 	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
-	_ui->odom_ratio->setObjectName(Parameters::kOdomWordsRatio().c_str());
-	_ui->odom_bin_nn->setObjectName(Parameters::kOdomNearestNeighbor().c_str());
-	_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomNNDR().c_str());
+	_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
+	_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
+
+	//Odometry BOW
+	_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());
+	_ui->odom_bin_nn->setObjectName(Parameters::kOdomBowNNType().c_str());
+	_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomBowNNDR().c_str());
+
+	//Odometry Optical Flow
+	_ui->odom_flow_winSize->setObjectName(Parameters::kOdomFlowWinSize().c_str());
+	_ui->odom_flow_maxLevel->setObjectName(Parameters::kOdomFlowMaxLevel().c_str());
+	_ui->odom_flow_iterations->setObjectName(Parameters::kOdomFlowIterations().c_str());
+	_ui->odom_flow_eps->setObjectName(Parameters::kOdomFlowEps().c_str());
+	_ui->odom_flow_subpix_winSize->setObjectName(Parameters::kOdomFlowSubPixWinSize().c_str());
+	_ui->odom_flow_subpix_iterations->setObjectName(Parameters::kOdomFlowSubPixIterations().c_str());
+	_ui->odom_flow_subpix_eps->setObjectName(Parameters::kOdomFlowSubPixEps().c_str());
 
 	setupSignals();
 	// custom signals
@@ -2797,6 +2811,10 @@ double PreferencesDialog::getVpThr() const
 {
 	return _ui->general_doubleSpinBox_vp->value();
 }
+int PreferencesDialog::getOdomStrategy() const
+{
+	return _ui->odom_strategy->currentIndex();
+}
 
 bool PreferencesDialog::isImagesKept() const
 {
@@ -2982,7 +3000,15 @@ void PreferencesDialog::testOdometry(int type)
 	if(camera)
 	{
 		ParametersMap parameters = this->getAllParameters();
-		Odometry * odometry = new OdometryBOW(parameters);
+		Odometry * odometry;
+		if(this->getOdomStrategy() == 1)
+		{
+			odometry = new OdometryOpticalFlow(parameters);
+		}
+		else
+		{
+			odometry = new OdometryBOW(parameters);
+		}
 
 		_odomThread = new OdometryThread(odometry); // take ownership of odometry
 

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>744</width>
-            <height>974</height>
+            <height>1019</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>24</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3281,7 +3281,7 @@ generate the number of words requested.</string>
                       <item row="6" column="1">
                        <widget class="QLabel" name="label_101">
                         <property name="text">
-                         <string>ROI ratios [left, right, top, bottom].</string>
+                         <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -5832,37 +5832,81 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                      </widget>
                     </item>
                     <item>
-                     <layout class="QFormLayout" name="formLayout_8">
-                      <property name="fieldGrowthPolicy">
-                       <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                      </property>
-                      <item row="1" column="0">
-                       <widget class="QLabel" name="label_44">
+                     <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_47">
                         <property name="text">
-                         <string>Feature detector </string>
+                         <string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="odom_localHistory">
+                      <item row="0" column="0">
+                       <widget class="QComboBox" name="odom_strategy">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContents</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>BOW (bag-of-words)</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>Optical Flow</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_46">
+                        <property name="text">
+                         <string>Angular update: minimum angular distance to update the odometry.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QSpinBox" name="odom_refine_iterations">
                         <property name="minimum">
                          <number>0</number>
                         </property>
                         <property name="maximum">
-                         <number>999999</number>
+                         <number>10000</number>
                         </property>
                         <property name="singleStep">
                          <number>1</number>
                         </property>
                         <property name="value">
-                         <number>0</number>
+                         <number>10</number>
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_111">
                         <property name="text">
-                         <string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
+                         <string>Refine iterations of the resulting transformation computed by RANSAC. 0 means no refining.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QSpinBox" name="odom_maxFeatures">
+                        <property name="maximum">
+                         <number>999999</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_136">
+                        <property name="text">
+                         <string>Max features extracted from the images (0 means inf).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -5870,32 +5914,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                        </widget>
                       </item>
                       <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_linearUpdate">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.015000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_45">
-                        <property name="text">
-                         <string>Linear update: minimum linear distance to update the odometry.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="4" column="0">
                        <widget class="QDoubleSpinBox" name="odom_angularUpdate">
                         <property name="suffix">
                          <string> m</string>
@@ -5911,96 +5929,10 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="1">
-                       <widget class="QLabel" name="label_46">
-                        <property name="text">
-                         <string>Angular update: minimum angular distance to update the odometry.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
-                       <widget class="QSpinBox" name="odom_countdown">
-                        <property name="maximum">
-                         <number>999999</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_47">
-                        <property name="text">
-                         <string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="0">
-                       <widget class="QSpinBox" name="odom_maxFeatures">
-                        <property name="maximum">
-                         <number>999999</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_136">
-                        <property name="text">
-                         <string>Max features extracted from the images (0 means inf).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="7" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_inlierDistance">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.005000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_149">
                         <property name="text">
-                         <string>Maximum distance for visual word correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="0">
-                       <widget class="QSpinBox" name="odom_minInliers">
-                        <property name="minimum">
-                         <number>8</number>
-                        </property>
-                        <property name="maximum">
-                         <number>1000</number>
-                        </property>
-                        <property name="value">
-                         <number>10</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="1">
-                       <widget class="QLabel" name="label_145">
-                        <property name="text">
-                         <string>Minimum visual word correspondences to compute geometry transform.</string>
+                         <string>RANSAC: Maximum distance for 3D feature correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6023,17 +5955,14 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
-                       <widget class="QLabel" name="label_36">
-                        <property name="text">
-                         <string>Maximum iterations to compute the transform from visual words.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
+                      <item row="4" column="0">
+                       <widget class="QSpinBox" name="odom_countdown">
+                        <property name="maximum">
+                         <number>999999</number>
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="11" column="0">
                        <widget class="QDoubleSpinBox" name="odom_ratio">
                         <property name="suffix">
                          <string/>
@@ -6055,7 +5984,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="1">
+                      <item row="11" column="1">
                        <widget class="QLabel" name="label_90">
                         <property name="text">
                          <string>Minmum ratio of keypoints between the current image and the last image to compute odometry.</string>
@@ -6065,7 +5994,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="11" column="0">
+                      <item row="12" column="0">
                        <widget class="QDoubleSpinBox" name="odom_maxDepth">
                         <property name="suffix">
                          <string> m</string>
@@ -6084,7 +6013,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="11" column="1">
+                      <item row="12" column="1">
                        <widget class="QLabel" name="label_104">
                         <property name="text">
                          <string>Max feature depth.</string>
@@ -6094,49 +6023,75 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
-                       <widget class="QComboBox" name="odom_bin_nn">
-                        <property name="sizeAdjustPolicy">
-                         <enum>QComboBox::AdjustToContents</enum>
-                        </property>
-                        <item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_145">
                         <property name="text">
-                          <string>FLANN Linear</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>FLANN KdTree</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>FLANN LSH</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>Brute Force</string>
-                         </property>
-                        </item>
-                        <item>
-                         <property name="text">
-                          <string>Brute Force GPU</string>
-                         </property>
-                        </item>
-                       </widget>
-                      </item>
-                      <item row="12" column="1">
-                       <widget class="QLabel" name="label_103">
-                        <property name="text">
-                         <string>Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector.</string>
+                         <string>Minimum feature correspondences to compute geometry transform.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_36">
+                        <property name="text">
+                         <string>RANSAC: Maximum iterations to compute the transform from 3D features.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QSpinBox" name="odom_minInliers">
+                        <property name="minimum">
+                         <number>8</number>
+                        </property>
+                        <property name="maximum">
+                         <number>1000</number>
+                        </property>
+                        <property name="value">
+                         <number>10</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_linearUpdate">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.015000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_inlierDistance">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.005000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
                        <widget class="QComboBox" name="odom_type">
                         <property name="sizeAdjustPolicy">
                          <enum>QComboBox::AdjustToContents</enum>
@@ -6183,34 +6138,55 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </item>
                        </widget>
                       </item>
-                      <item row="13" column="1">
-                       <widget class="QLabel" name="label_123">
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_45">
                         <property name="text">
-                         <string>NNDR ratio
-(A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor)
-Lower the ratio -&gt; higher the precision. 0 means disabled, matching the nearest.</string>
+                         <string>Linear update: minimum linear distance to update the odometry.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_bin_nndrRatio">
-                        <property name="decimals">
-                         <number>1</number>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_44">
+                        <property name="text">
+                         <string>Feature detector. In BOW mode, the related descriptor is also used. In Optical flow mode, only the keypoint detector is used.</string>
                         </property>
-                        <property name="minimum">
-                         <double>0.100000000000000</double>
+                        <property name="wordWrap">
+                         <bool>true</bool>
                         </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_103">
+                        <property name="text">
+                         <string>Odometry strategy:
+1-BOW matches features extracted from both frames using nearest neighbor with descriptors, then computes RANSAC transformation estimation with corresponding 3D features.
+2-Optical flow estimate the location of 2D features from last frame to new frame, then computes RANSAC transformation with corresponding 3D features.</string>
                         </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
+                        <property name="wordWrap">
+                         <bool>true</bool>
                         </property>
-                        <property name="value">
-                         <double>0.700000000000000</double>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QLineEdit" name="lineEdit_odom_roi">
+                        <property name="text">
+                         <string>0.0 0.0 0.0 0.0</string>
+                        </property>
+                        <property name="readOnly">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_123">
+                        <property name="text">
+                         <string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
                         </property>
                        </widget>
                       </item>
@@ -6258,6 +6234,378 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_26">
+                <layout class="QVBoxLayout" name="verticalLayout_54">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_odometryBOW3">
+                   <property name="title">
+                    <string>BOW</string>
+                   </property>
+                   <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,1">
+                    <item row="0" column="0">
+                     <widget class="QSpinBox" name="odom_localHistory">
+                      <property name="minimum">
+                       <number>0</number>
+                      </property>
+                      <property name="maximum">
+                       <number>999999</number>
+                      </property>
+                      <property name="singleStep">
+                       <number>1</number>
+                      </property>
+                      <property name="value">
+                       <number>0</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="1">
+                     <widget class="QLabel" name="label_190">
+                      <property name="text">
+                       <string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="0">
+                     <widget class="QComboBox" name="odom_bin_nn">
+                      <property name="sizeAdjustPolicy">
+                       <enum>QComboBox::AdjustToContents</enum>
+                      </property>
+                      <item>
+                       <property name="text">
+                        <string>FLANN Linear</string>
+                       </property>
+                      </item>
+                      <item>
+                       <property name="text">
+                        <string>FLANN KdTree</string>
+                       </property>
+                      </item>
+                      <item>
+                       <property name="text">
+                        <string>FLANN LSH</string>
+                       </property>
+                      </item>
+                      <item>
+                       <property name="text">
+                        <string>Brute Force</string>
+                       </property>
+                      </item>
+                      <item>
+                       <property name="text">
+                        <string>Brute Force GPU</string>
+                       </property>
+                      </item>
+                     </widget>
+                    </item>
+                    <item row="1" column="1">
+                     <widget class="QLabel" name="label_201">
+                      <property name="text">
+                       <string>Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="0">
+                     <widget class="QDoubleSpinBox" name="odom_bin_nndrRatio">
+                      <property name="decimals">
+                       <number>1</number>
+                      </property>
+                      <property name="minimum">
+                       <double>0.100000000000000</double>
+                      </property>
+                      <property name="maximum">
+                       <double>1.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.100000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>0.700000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="1">
+                     <widget class="QLabel" name="label_202">
+                      <property name="text">
+                       <string>NNDR ratio
+(A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor)
+Lower the ratio -&gt; higher the precision. 0 means disabled, matching the nearest.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_27">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>670</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
+               <widget class="QWidget" name="page_28">
+                <layout class="QVBoxLayout" name="verticalLayout_55">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_odometryFlow3">
+                   <property name="title">
+                    <string>Optical Flow</string>
+                   </property>
+                   <layout class="QVBoxLayout" name="verticalLayout_53">
+                    <item>
+                     <widget class="QLabel" name="label_199">
+                      <property name="text">
+                       <string>The process is as follow:
+  - Features from the last frame are estimated in the new frame using an optical flow approach (see cv::calcOpticalFlowPyrLK()).
+  - 3D features from the new frame are extracted from the estimated positions.
+  - Using RANSAC, a transformation is estimated between corresponding 3D features.
+  - New features are extracted from the new frame to be used for the next time.
+  - Optionally, the 2D position of the features can be refined for sub pixel precision (see cv::cornerSubPix()). </string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_4">
+                      <property name="title">
+                       <string>calcOpticalFlowPyrLK()</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_30" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="odom_flow_winSize">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>21</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_192">
+                         <property name="text">
+                          <string>Window size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="odom_flow_iterations">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>30</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_193">
+                         <property name="text">
+                          <string>Iterations.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="odom_flow_eps">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.001000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_194">
+                         <property name="text">
+                          <string>Epsilon.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_195">
+                         <property name="text">
+                          <string>Max level.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="odom_flow_maxLevel">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>3</number>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_7">
+                      <property name="title">
+                       <string>cornerSubPix()</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_31" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="odom_flow_subpix_winSize">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>5</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_196">
+                         <property name="text">
+                          <string>Window size for sub pixel estimation.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="odom_flow_subpix_iterations">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>20</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_197">
+                         <property name="text">
+                          <string>Iterations for sub pixel estimation. 0 disables sub pixel refining.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="odom_flow_subpix_eps">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.001000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.030000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_198">
+                         <property name="text">
+                          <string>Epsilon for sub pixel estimation.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_28">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>518</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
               </widget>
              </item>
             </layout>

tools/OdometryViewer/main.cpp

@@ -36,16 +36,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/DBReader.h>
 #include <rtabmap/core/VWDictionary.h>
 #include <QtGui/QApplication>
+#include <pcl/console/print.h>
 
 void showUsage()
 {
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF\n"
+			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n"
+			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
 			"  -nn #                     Nearest neighbor strategy (default 1): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
 			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
 			"  -icp                      Use ICP odometry\n"
+			"  -flow                     Use optical flow odometry.\n"
 			"\n"
 			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
 			"  -db \"input.db\"          Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
@@ -80,7 +83,8 @@ void showUsage()
 			"  odometryViewer -odom 1 -lh 10000                     SIFT example\n"
 			"  odometryViewer -odom 4 -nn 2 -lh 1000                FAST/BRIEF example\n"
 			"  odometryViewer -odom 3 -nn 2 -lh 1000                FAST/FREAK example\n"
-			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n");
+			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n"
+			"  odometryViewer -flow                                 Optical flow example\n");
 	exit(1);
 }
 
@@ -92,8 +96,10 @@ int main (int argc, char * argv[])
 	// parse arguments
 	float rate = 0.0;
 	std::string inputDatabase;
+	int driver = 0;
 	int odomType = 0;
 	bool icp = false;
+	bool flow = false;
 	int nnType =1;
 	float nndr = 0.7f;
 	float distance = 0.005;
@@ -119,6 +125,23 @@ int main (int argc, char * argv[])
 
 	for(int i=1; i<argc; ++i)
 	{
+		if(strcmp(argv[i], "-driver") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				driver = std::atoi(argv[i]);
+				if(driver < 0 || driver > 4)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
 		if(strcmp(argv[i], "-o") == 0)
 		{
 			++i;
@@ -521,6 +544,11 @@ int main (int argc, char * argv[])
 			icp = true;
 			continue;
 		}
+		if(strcmp(argv[i], "-flow") == 0)
+		{
+			flow = true;
+			continue;
+		}
 		if(strcmp(argv[i], "-p2p") == 0)
 		{
 			p2p = true;
@@ -585,12 +613,21 @@ int main (int argc, char * argv[])
 	{
 		odomName = "GFTT+BRIEF";
 	}
+	else if(odomType == 7)
+	{
+		odomName = "BRISK";
+	}
 
 	if(icp)
 	{
 		odomName= "ICP";
 	}
 
+	if(flow)
+	{
+		odomName= "Optical Flow";
+	}
+
 	std::string nnName;
 	if(nnType == 0)
 	{
@@ -633,10 +670,24 @@ int main (int argc, char * argv[])
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLinearUpdate(), uNumber2Str(linearUpdate)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomAngularUpdate(), uNumber2Str(angularUpdate)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLocalHistory(), uNumber2Str(localHistory)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
 
 	if(!icp)
 	{
+		if(flow)
+		{
+			// Optical Flow
+			UINFO("Min inliers =             %d", minInliers);
+			UINFO("Inlier maximum correspondences distance = %f", distance);
+			UINFO("RANSAC iterations =       %d", iterations);
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
+			odom = new rtabmap::OdometryOpticalFlow(parameters);
+		}
+		else
+		{
+			//BOW
 			UINFO("Nearest neighbor =         %s", nnName.c_str());
 			UINFO("Nearest neighbor ratio =  %f", nndr);
 			UINFO("Max features =            %d", maxWords);
@@ -645,14 +696,14 @@ int main (int argc, char * argv[])
 			UINFO("Inlier maximum correspondences distance = %f", distance);
 			UINFO("RANSAC iterations =       %d", iterations);
 			UINFO("GPU =                     %s", gpu?"true":"false");
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxWords(), uNumber2Str(maxWords)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomWordsRatio(), uNumber2Str(wordsRatio)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomNearestNeighbor(), uNumber2Str(nnType)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomNNDR(), uNumber2Str(nndr)));
-		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomType(), uNumber2Str(odomType)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeaturesRatio(), uNumber2Str(wordsRatio)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
 			if(odomType == 0)
 			{
 				parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
@@ -675,6 +726,7 @@ int main (int argc, char * argv[])
 
 			odom = new rtabmap::OdometryBOW(parameters);
 		}
+	}
 	else // ICP
 	{
 		UINFO("ICP maximum correspondences distance = %f", distance);
@@ -718,15 +770,64 @@ int main (int argc, char * argv[])
 	}
 	else
 	{
-		rtabmap::CameraThread camera(new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
-		if(camera.init())
+		rtabmap::CameraRGBD * camera = 0;
+		rtabmap::Transform t=rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+		if(driver == 0)
+		{
+			camera = new rtabmap::CameraOpenni("", rate, t);
+		}
+		else if(driver == 1)
+		{
+			if(!rtabmap::CameraOpenNI2::available())
+			{
+				UERROR("Not built with OpenNI2 support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNI2(rate, t);
+		}
+		else if(driver == 2)
+		{
+			if(!rtabmap::CameraFreenect::available())
+			{
+				UERROR("Not built with Freenect support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraFreenect(0, rate, t);
+		}
+		else if(driver == 3)
+		{
+			if(!rtabmap::CameraOpenNICV::available())
+			{
+				UERROR("Not built with OpenNI from OpenCV support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNICV(false, rate, t);
+		}
+		else if(driver == 4)
+		{
+			if(!rtabmap::CameraOpenNICV::available())
+			{
+				UERROR("Not built with OpenNI from OpenCV support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNICV(true, rate, t);
+		}
+		else
+		{
+			UFATAL("Camera driver (%d) not found!", driver);
+		}
+
+		//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
+
+		rtabmap::CameraThread cameraThread(camera);
+		if(cameraThread.init())
 		{
 			odomThread.start();
-			camera.start();
+			cameraThread.start();
 
 			app.exec();
 
-			camera.kill();
+			cameraThread.kill();
 			odomThread.join(true);
 		}
 	}

utilite/include/rtabmap/utilite/UMath.h

@@ -860,4 +860,10 @@ inline std::vector<float> uHamming(unsigned int L)
 	return w;
 }
 
+template <typename T>
+bool uIsInBounds(const T& value, const T& low, const T& high)
+{
+	return !(value < low) && !(value > high);
+}
+
 #endif // UMATH_H

utilite/include/rtabmap/utilite/UStl.h

@@ -622,4 +622,12 @@ inline int uStrNumCmp(const std::string & a, const std::string & b)
 	return result;
 }
 
+/**
+ * Check if a string contains a specified substring.
+ */
+inline bool uStrContains(const std::string & string, const std::string & substring)
+{
+	return string.find(substring) != std::string::npos;
+}
+
 #endif /* USTL_H */