Added Stereo and StereoDense base classes (with Stereo->StereoOpticalFlow and StereoDense->StereoBM) to handle easily stereo parameters. Added stereoEval tool to test Stereo/OpticalFlow=false or true. Added StereoBM parameters. Refactoring of the Preferences dialog (tree view order and some titles)

corelib/include/rtabmap/core/CameraModel.h

@@ -125,89 +125,5 @@ private:
 	Transform localTransform_;
 };
 
-class RTABMAP_EXP StereoCameraModel
-{
-public:
-	StereoCameraModel() {}
-	StereoCameraModel(
-			const std::string & name,
-			const cv::Size & imageSize1,
-			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
-			const cv::Size & imageSize2,
-			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
-			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
-		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
-		name_(name),
-		R_(R),
-		T_(T),
-		E_(E),
-		F_(F)
-	{
-	}
-	//minimal
-	StereoCameraModel(
-			double fx,
-			double fy,
-			double cx,
-			double cy,
-			double baseline,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(fx, fy, cx, cy, localTransform),
-		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
-	{
-	}
-	//minimal to be saved
-	StereoCameraModel(
-			const std::string & name,
-			double fx,
-			double fy,
-			double cx,
-			double cy,
-			double baseline,
-			const Transform & localTransform = Transform::getIdentity()) :
-		left_(name+"_left", fx, fy, cx, cy, localTransform),
-		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
-		name_(name)
-	{
-	}
-	virtual ~StereoCameraModel() {}
-
-	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
-
-	void setName(const std::string & name);
-	const std::string & name() const {return name_;}
-
-	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
-	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
-
-	double baseline() const {return -right_.Tx()/right_.fx();}
-
-	const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
-	const cv::Mat & T() const {return T_;} //extrinsic translation matrix
-	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
-	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
-
-	void scale(double scale);
-
-	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
-	const Transform & localTransform() const {return left_.localTransform();}
-	Transform stereoTransform() const;
-
-	const CameraModel & left() const {return left_;}
-	const CameraModel & right() const {return right_;}
-
-private:
-	CameraModel left_;
-	CameraModel right_;
-	std::string name_;
-	cv::Mat R_;
-	cv::Mat T_;
-	cv::Mat E_;
-	cv::Mat F_;
-};
-
-
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */

corelib/include/rtabmap/core/CameraThread.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 
@@ -36,6 +37,7 @@ namespace rtabmap
 {
 
 class Camera;
+class StereoDense;
 
 /**
  * Class CameraThread
@@ -47,7 +49,7 @@ class RTABMAP_EXP CameraThread :
 {
 public:
 	// ownership transferred
-	CameraThread(Camera * camera);
+	CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
@@ -70,6 +72,7 @@ private:
 	bool _mirroring;
 	bool _colorOnly;
 	bool _stereoToDepth;
+	StereoDense * _stereoDense;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Features2d.h

@@ -102,7 +102,7 @@ public:
 		kFeatureGfttOrb=8,   //new 0.10.11
 		kFeatureFastOrb=9};  //new 0.11.0
 
-	static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
+	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters);
 
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,

corelib/include/rtabmap/core/Memory.h

@@ -54,6 +54,7 @@ class Feature2D;
 class Statistics;
 class RegistrationVis;
 class RegistrationIcp;
+class Stereo;
 
 class RTABMAP_EXP Memory
 {
@@ -273,11 +274,7 @@ private:
 	RegistrationIcp * _registrationIcp;
 
 	// Stereo stuff
-	int _stereoFlowWinSize;
-	int _stereoFlowIterations;
-	double _stereoFlowEpsilon;
-	int _stereoFlowMaxLevel;
-	float _stereoMaxSlope;
+	Stereo * _stereo;
 
 	int _subPixWinSize;
 	int _subPixIterations;

corelib/include/rtabmap/core/Odometry.h

@@ -43,6 +43,7 @@ namespace rtabmap {
 class Feature2D;
 class OdometryInfo;
 class ParticleFilter;
+class Stereo;
 
 class RTABMAP_EXP Odometry
 {
@@ -143,16 +144,14 @@ private:
 
 private:
 	//Parameters:
+	int keyFrameThr_;
 	int flowWinSize_;
 	int flowIterations_;
 	double flowEps_;
 	int flowMaxLevel_;
+	bool flowGuessFromMotion_;
 
-	int stereoWinSize_;
-	int stereoIterations_;
-	double stereoEps_;
-	int stereoMaxLevel_;
-	float stereoMaxSlope_;
+	Stereo * stereo_;
 
 	int subPixWinSize_;
 	int subPixIterations_;
@@ -163,6 +162,8 @@ private:
 	cv::Mat refFrame_;
 	std::vector<cv::Point2f> refCorners_;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
+
+	Transform motionSinceLastKeyFrame_;
 };
 
 class RTABMAP_EXP OdometryMono : public Odometry
@@ -181,11 +182,7 @@ private:
 	double flowEps_;
 	int flowMaxLevel_;
 
-	int stereoWinSize_;
-	int stereoIterations_;
-	double stereoEps_;
-	int stereoMaxLevel_;
-	float stereoMaxSlope_;
+	Stereo * stereo_;
 
 	Memory * memory_;
 	int localHistoryMaxSize_;

corelib/include/rtabmap/core/Parameters.h

@@ -347,11 +347,13 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
 	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01, "Maximum variance to add new points to local map.");
 
-	// Odometry common stuff between BOW and Optical Flow approaches
+	// Odometry Optical Flow
+	RTABMAP_PARAM(OdomFlow, KeyFrameThr,           int, 0,        "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, Iterations,            int, 30,       "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, Eps,                   double, 0.01,  "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
 	RTABMAP_PARAM(OdomFlow, MaxLevel,              int, 3,        "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(OdomFlow, GuessMotion,           bool, true,    "Guess optical flow from the last motion computed.");
 
 	// Common registration parameters
 	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
@@ -391,11 +393,26 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
 
 	// Stereo disparity
-	RTABMAP_PARAM(Stereo, WinSize,               int, 16,       "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "See cv::calcOpticalFlowPyrLK().");
-	RTABMAP_PARAM(Stereo, MaxSlope,              float, 0.1,    "The maximum slope for each stereo pairs.");
+	RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");
+	RTABMAP_PARAM(Stereo, WinHeight,             int, 3,        "Window height.");
+	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
+	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
+	RTABMAP_PARAM(Stereo, MinDisparity,          int, 0,        "Minimum disparity.");
+	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 64,       "Maximum disparity.");
+	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
+	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
+	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
+	RTABMAP_PARAM(Stereo, MaxSlope,              float, 0.1,    "[Stereo/OpticalFlow = true] The maximum slope for each stereo pairs.");
+
+	RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, NumDisparities,      int, 64,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, PreFilterSize,       int, 9,        "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, PreFilterCap,        int, 31,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, UniquenessRatio,     int, 15,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, TextureThreshold,    int, 10,       "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, SpeckleWindowSize,   int, 100,      "See cv::StereoBM");
+	RTABMAP_PARAM(StereoBM, SpeckleRange,        int, 4,        "See cv::StereoBM");
 
 public:
 	virtual ~Parameters();
@@ -421,12 +438,14 @@ public:
 	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
 	static void parse(const ParametersMap & parameters, const std::string & key, double & value);
 	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
+	static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
 
 	static std::string getVersion();
 	static std::string getDefaultDatabaseName();
 	
 	static bool isFeatureParameter(const std::string & param);
 	static ParametersMap getDefaultOdometryParameters(bool stereo = false);
+	static ParametersMap getDefaultParameters(const std::string & group);
 
 	static void readINI(const std::string & configFile, ParametersMap & parameters);
 	static void writeINI(const std::string & configFile, const ParametersMap & parameters);

corelib/include/rtabmap/core/SensorData.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>

corelib/include/rtabmap/core/Stereo.h

@@ -0,0 +1,89 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef STEREO_H_
+#define STEREO_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP Stereo {
+public:
+	Stereo(const ParametersMap & parameters = ParametersMap());
+	virtual ~Stereo() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+
+	cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
+	int iterations() const   {return iterations_;}
+	int maxLevel() const     {return maxLevel_;}
+	int minDisparity() const {return minDisparity_;}
+	int maxDisparity() const {return maxDisparity_;}
+	bool winSSD() const      {return winSSD_;}
+
+private:
+	int winWidth_;
+	int winHeight_;
+	int iterations_;
+	int maxLevel_;
+	int minDisparity_;
+	int maxDisparity_;
+	bool winSSD_;
+};
+
+class RTABMAP_EXP StereoOpticalFlow : public Stereo {
+public:
+	StereoOpticalFlow(const ParametersMap & parameters = ParametersMap());
+	virtual ~StereoOpticalFlow() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+
+	float epsilon() const {return epsilon_;}
+	float maxSlope() const {return maxSlope_;}
+
+private:
+	float epsilon_;
+	float maxSlope_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREO_H_ */

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -0,0 +1,143 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef STEREOCAMERAMODEL_H_
+#define STEREOCAMERAMODEL_H_
+
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP StereoCameraModel
+{
+public:
+	StereoCameraModel() {}
+	StereoCameraModel(
+			const std::string & name,
+			const cv::Size & imageSize1,
+			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
+			const cv::Size & imageSize2,
+			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
+			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
+		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
+		name_(name),
+		R_(R),
+		T_(T),
+		E_(E),
+		F_(F)
+	{
+	}
+	StereoCameraModel(
+			const std::string & name,
+			const CameraModel & leftCameraModel,
+			const CameraModel & rightCameraModel,
+			const cv::Mat & R = cv::Mat(),
+			const cv::Mat & T = cv::Mat(),
+			const cv::Mat & E = cv::Mat(),
+			const cv::Mat & F = cv::Mat()) :
+		left_(leftCameraModel),
+		right_(rightCameraModel),
+		name_(name),
+		R_(R),
+		T_(T),
+		E_(E),
+		F_(F)
+	{
+		left_.setName(name+"_left");
+		right_.setName(name+"_right");
+	}
+	//minimal
+	StereoCameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(fx, fy, cx, cy, localTransform),
+		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
+	{
+	}
+	//minimal to be saved
+	StereoCameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", fx, fy, cx, cy, localTransform),
+		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+		name_(name)
+	{
+	}
+	virtual ~StereoCameraModel() {}
+
+	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
+
+	void setName(const std::string & name);
+	const std::string & name() const {return name_;}
+
+	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
+	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+
+	double baseline() const {return -right_.Tx()/right_.fx();}
+
+	float computeDepth(float disparity) const;
+	float computeDisparity(float depth) const; // m
+	float computeDisparity(unsigned short depth) const; // mm
+
+	const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
+	const cv::Mat & T() const {return T_;} //extrinsic translation matrix
+	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
+	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
+
+	void scale(double scale);
+
+	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
+	const Transform & localTransform() const {return left_.localTransform();}
+	Transform stereoTransform() const;
+
+	const CameraModel & left() const {return left_;}
+	const CameraModel & right() const {return right_;}
+
+private:
+	CameraModel left_;
+	CameraModel right_;
+	std::string name_;
+	cv::Mat R_;
+	cv::Mat T_;
+	cv::Mat E_;
+	cv::Mat F_;
+};
+
+} // rtabmap
+
+#endif /* STEREOCAMERAMODEL_H_ */

corelib/include/rtabmap/core/StereoDense.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef STEREODENSE_H_
+#define STEREODENSE_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_EXP StereoDense {
+public:
+	virtual ~StereoDense() {}
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual cv::Mat computeDisparity(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage) const = 0;
+
+protected:
+	StereoDense(const ParametersMap & parameters = ParametersMap()) {}
+};
+
+class RTABMAP_EXP StereoBM : public StereoDense {
+public:
+	StereoBM(int blockSize, int numDisparities);
+	StereoBM(const ParametersMap & parameters = ParametersMap());
+	virtual ~StereoBM() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat computeDisparity(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage) const;
+
+private:
+	int blockSize_;         //15
+	int minDisparity_;      //0
+	int numDisparities_;    //64
+	int preFilterSize_;     //9
+	int preFilterCap_;      //31
+	int uniquenessRatio_;   //15
+	int textureThreshold_;  //10
+	int speckleWindowSize_; //100
+	int speckleRange_;      //4
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREODENSE_H_ */

corelib/include/rtabmap/core/util2d.h

@@ -39,20 +39,36 @@ namespace rtabmap
 namespace util2d
 {
 
-cv::Mat RTABMAP_EXP disparityFromStereoImages(
+// SSD: Sum of Squared Differences
+float RTABMAP_EXP ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight);
+// SAD: Sum of Absolute intensity Differences
+float RTABMAP_EXP sad(const cv::Mat & windowLeft, const cv::Mat & windowRight);
+
+std::vector<cv::Point2f> RTABMAP_EXP calcStereoCorrespondences(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status,
+		cv::Size winSize = cv::Size(6,3),
+		int maxLevel = 3,
+		int iterations = 5,
+		int minDisparity = 0,
+		int maxDisparity = 64,
+		bool ssdApproach = true); // SSD by default, otherwise it is SAD
+
+// exactly as cv::calcOpticalFlowPyrLK but it should be called with pyramid (from cv::buildOpticalFlowPyramid()) and delta drops the y error.
+void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
+                           cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
+                           cv::OutputArray _status, cv::OutputArray _err,
+                           cv::Size winSize = cv::Size(15,3), int maxLevel = 3,
+						   cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01),
+						   int flags = 0, double minEigThreshold = 1e-4 );
+
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02,
-		float maxCorrespondencesSlope = 0.1f);
+		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
 
 cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,
@@ -70,11 +86,10 @@ cv::Mat RTABMAP_EXP depthFromStereoImages(
 		double flowEps = 0.02);
 
 cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
-		const cv::Mat & leftImage,
+		const cv::Size & disparitySize,
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask,
-		float maxSlope = 0.1f);
+		const std::vector<unsigned char> & mask);
 
 cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
 		const cv::Mat & leftImage,

corelib/include/rtabmap/core/util3d.h

@@ -87,22 +87,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
 		const cv::Mat & imageDisparity,
-		float cx, float cy,
-		float fx, float baseline,
+		const StereoCameraModel & model,
 		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,
+		const StereoCameraModel & model,
 		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
-		float cx, float cy,
-		float fx, float baseline,
+		const StereoCameraModel & model,
 		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
@@ -136,12 +133,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,
-		float cx, float cy, float fx, float baseline);
+		const StereoCameraModel & model);
 
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		const cv::Mat & disparity,
-		float cx, float cy, float fx, float baseline);
+		const StereoCameraModel & model);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);

corelib/include/rtabmap/core/util3d_features.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d.hpp>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
 #include <list>
 #include <map>
 
@@ -60,34 +61,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
 		const cv::Mat & disparity,
 		const StereoCameraModel & stereoCameraMode);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		Transform localTransform = Transform::getIdentity(),
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02,
-		double maxCorrespondencesSlope = 0.0);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		const std::vector<cv::Point2f> & leftCorners,
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		Transform localTransform = Transform::getIdentity(),
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02,
-		double maxCorrespondencesSlope = 0.0);
+		const std::vector<cv::Point2f> & rightCorners,
+		const StereoCameraModel & model,
+		const std::vector<unsigned char> & mask = std::vector<unsigned char>());
 
 std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
 		const std::multimap<int, cv::KeyPoint> & kpts,

corelib/src/CMakeLists.txt

@@ -60,6 +60,10 @@ SET(SRC_FILES
 	OdometryMono.cpp
 	OdometryICP.cpp
 	
+	Stereo.cpp
+	StereoDense.cpp
+	StereoCameraModel.cpp
+	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp
 	toro3d/treeoptimizer3.cpp

corelib/src/CameraModel.cpp

@@ -362,151 +362,4 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 	}
 }
 
-//
-//StereoCameraModel
-//
-void StereoCameraModel::setName(const std::string & name)
-{
-	name_=name;
-	left_.setName(name_+"_left");
-	right_.setName(name_+"_right");
-}
-
-bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
-{
-	name_ = cameraName;
-	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
-	{
-		if(ignoreStereoTransform)
-		{
-			return true;
-		}
-		//load rotation, translation
-		R_ = cv::Mat();
-		T_ = cv::Mat();
-
-		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
-		if(UFile::exists(filePath))
-		{
-			UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
-			cv::FileStorage fs(filePath, cv::FileStorage::READ);
-
-			name_ = (int)fs["camera_name"];
-
-			// import from ROS calibration format
-			cv::FileNode n = fs["rotation_matrix"];
-			int rows = (int)n["rows"];
-			int cols = (int)n["cols"];
-			std::vector<double> data;
-			n["data"] >> data;
-			UASSERT(rows*cols == (int)data.size());
-			UASSERT(rows == 3 && cols == 3);
-			R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-			n = fs["translation_matrix"];
-			rows = (int)n["rows"];
-			cols = (int)n["cols"];
-			data.clear();
-			n["data"] >> data;
-			UASSERT(rows*cols == (int)data.size());
-			UASSERT(rows == 3 && cols == 1);
-			T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-			n = fs["essential_matrix"];
-			rows = (int)n["rows"];
-			cols = (int)n["cols"];
-			data.clear();
-			n["data"] >> data;
-			UASSERT(rows*cols == (int)data.size());
-			UASSERT(rows == 3 && cols == 3);
-			E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-			n = fs["fundamental_matrix"];
-			rows = (int)n["rows"];
-			cols = (int)n["cols"];
-			data.clear();
-			n["data"] >> data;
-			UASSERT(rows*cols == (int)data.size());
-			UASSERT(rows == 3 && cols == 3);
-			F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-
-			fs.release();
-
-			return true;
-		}
-		else
-		{
-			UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
-		}
-	}
-	return false;
-}
-bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
-{
-	if(left_.save(directory) && right_.save(directory))
-	{
-		if(ignoreStereoTransform)
-		{
-			return true;
-		}
-		std::string filePath = directory+"/"+name_+"_pose.yaml";
-		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
-		{
-			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
-			cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
-
-			// export in ROS calibration format
-
-			fs << "camera_name" << name_;
-
-			fs << "rotation_matrix" << "{";
-			fs << "rows" << R_.rows;
-			fs << "cols" << R_.cols;
-			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
-			fs << "}";
-
-			fs << "translation_matrix" << "{";
-			fs << "rows" << T_.rows;
-			fs << "cols" << T_.cols;
-			fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
-			fs << "}";
-
-			fs << "essential_matrix" << "{";
-			fs << "rows" << E_.rows;
-			fs << "cols" << E_.cols;
-			fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
-			fs << "}";
-
-			fs << "fundamental_matrix" << "{";
-			fs << "rows" << F_.rows;
-			fs << "cols" << F_.cols;
-			fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
-			fs << "}";
-
-			fs.release();
-
-			return true;
-		}
-	}
-	return false;
-}
-
-void StereoCameraModel::scale(double scale)
-{
-	left_.scale(scale);
-	right_.scale(scale);
-}
-
-Transform StereoCameraModel::stereoTransform() const
-{
-	if(!R_.empty() && !T_.empty())
-	{
-		return Transform(
-				R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
-				R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
-				R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
-	}
-	return Transform();
-}
-
 } /* namespace rtabmap */

corelib/src/CameraThread.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/StereoDense.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -39,11 +40,12 @@ namespace rtabmap
 {
 
 // ownership transferred
-CameraThread::CameraThread(Camera * camera) :
+CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_camera(camera),
 		_mirroring(false),
 		_colorOnly(false),
-		_stereoToDepth(false)
+		_stereoToDepth(false),
+		_stereoDense(new StereoBM(parameters))
 {
 	UASSERT(_camera != 0);
 }
@@ -55,6 +57,7 @@ CameraThread::~CameraThread()
 	{
 		delete _camera;
 	}
+	delete _stereoDense;
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -105,7 +108,7 @@ void CameraThread::mainLoop()
 		{
 			UTimer timer;
 			cv::Mat depth = util2d::depthFromDisparity(
-					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
+					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
 					data.stereoCameraModel().left().fx(),
 					data.stereoCameraModel().baseline());
 			data.setCameraModel(data.stereoCameraModel().left());

corelib/src/Features2d.cpp

@@ -343,7 +343,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 {
 	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
 }
-Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
+Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
 {
 	if(RTABMAP_NONFREE == 0)
 	{

corelib/src/Memory.cpp

@@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
+#include "rtabmap/core/Stereo.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
@@ -104,20 +105,16 @@ Memory::Memory(const ParametersMap & parameters) :
 	_wordsMinDepth(Parameters::defaultKpMinDepth()),
 	_roiRatios(std::vector<float>(4, 0.0f)),
 
-	_stereoFlowWinSize(Parameters::defaultStereoWinSize()),
-	_stereoFlowIterations(Parameters::defaultStereoIterations()),
-	_stereoFlowEpsilon(Parameters::defaultStereoEps()),
-	_stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()),
-	_stereoMaxSlope(Parameters::defaultStereoMaxSlope()),
-
 	_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
 	_subPixIterations(Parameters::defaultKpSubPixIterations()),
 	_subPixEps(Parameters::defaultKpSubPixEps())
 {
 	_feature2D = Feature2D::create(_featureType, parameters);
+	_featureType = _feature2D->getType();
 	_vwd = new VWDictionary(parameters);
 	_registrationVis = new RegistrationVis(parameters);
 	_registrationIcp = new RegistrationIcp(parameters);
+	_stereo = new Stereo(parameters);
 	this->parseParameters(parameters);
 }
 
@@ -377,6 +374,10 @@ Memory::~Memory()
 	{
 		delete _registrationIcp;
 	}
+	if(_stereo)
+	{
+		delete _stereo;
+	}
 	if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
 }
 
@@ -417,13 +418,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_dbDriver->parseParameters(parameters);
 	}
 
-	//stereo
-	Parameters::parse(parameters, Parameters::kStereoWinSize(), _stereoFlowWinSize);
-	Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations);
-	Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon);
-	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel);
-	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), _stereoMaxSlope);
-
 	// Keypoint stuff
 	if(_vwd)
 	{
@@ -463,7 +457,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 
 		_feature2D = Feature2D::create(detectorStrategy, parameters);
-		_featureType = detectorStrategy;
+		_featureType = _feature2D->getType();
 	}
 	else if(_feature2D)
 	{
@@ -477,7 +471,27 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	if(_registrationIcp)
 	{
 		_registrationIcp->parseParameters(parameters);
-	}
+	}
+
+	//stereo
+	UASSERT(_stereo != 0);
+	if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
+	{
+		bool opticalFlow = uStr2Bool(iter->second);
+		delete _stereo;
+		if(opticalFlow)
+		{
+			_stereo = new StereoOpticalFlow(parameters);
+		}
+		else
+		{
+			_stereo = new Stereo(parameters);
+		}
+	}
+	else
+	{
+		_stereo->parseParameters(parameters);
+	}
 
 	// do this after all parameters are parsed
 	// SLAM mode vs Localization mode
@@ -3093,7 +3107,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid())
 			{
 				//stereo
-				cv::Mat disparity;
 				bool subPixelOn = false;
 				if(_subPixWinSize > 0 && _subPixIterations > 0)
 				{
@@ -3137,27 +3150,35 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 					}
 
 					//generate a disparity map
-					disparity = util2d::disparityFromStereoImages(
+					std::vector<unsigned char> status;
+					std::vector<cv::Point2f> rightCorners;
+					rightCorners = _stereo->computeCorrespondences(
 							imageMono,
-							data.depthOrRightRaw(),
+							data.rightRaw(),
 							leftCorners,
-							_stereoFlowWinSize,
-							_stereoFlowMaxLevel,
-							_stereoFlowIterations,
-							_stereoFlowEpsilon,
-							_stereoMaxSlope);
+							status);
+					if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
+					{
+						UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
+						for(unsigned int i=0; i<status.size(); ++i)
+						{
+							if(status[i] != 0)
+							{
+								float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
+								if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
+								   (_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
+								{
+									status[i] = 0;
+								}
+							}
+						}
+					}
+
+
 					t = timer.ticks();
 					if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
 					UDEBUG("generate disparity = %fs", t);
 
-					if(_wordsMaxDepth > 0.0f)
-					{
-						// disparity = baseline * fx / depth;
-						float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth;
-						Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
-						UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
-					}
-
 					if(keypoints.size())
 					{
 						if(!subPixelOn)
@@ -3167,11 +3188,13 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 							if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 							UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 						}
-
-						keypoints3D = util3d::generateKeypoints3DDisparity(
-								keypoints,
-								disparity,
-								data.stereoCameraModel());
+
+						keypoints3D = util3d::generateKeypoints3DStereo(
+								leftCorners,
+								rightCorners,
+								data.stereoCameraModel(),
+								status);
+
 						t = timer.ticks();
 						if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 						UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
@@ -3323,30 +3346,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			//generate a disparity map
 			std::vector<cv::Point2f> leftCorners;
 			cv::KeyPoint::convert(keypoints, leftCorners);
-			cv::Mat disparity = util2d::disparityFromStereoImages(
+			std::vector<unsigned char> status;
+
+			std::vector<cv::Point2f> rightCorners;
+			rightCorners = _stereo->computeCorrespondences(
 					imageMono,
-					data.depthOrRightRaw(),
+					data.rightRaw(),
 					leftCorners,
-					_stereoFlowWinSize,
-					_stereoFlowMaxLevel,
-					_stereoFlowIterations,
-					_stereoFlowEpsilon,
-					_stereoMaxSlope);
+					status);
+
+			if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
+			{
+				UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
+				for(unsigned int i=0; i<status.size(); ++i)
+				{
+					if(status[i] != 0)
+					{
+						float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
+						if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
+						   (_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
+						{
+							status[i] = 0;
+						}
+					}
+				}
+			}
+
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
 			UDEBUG("generate disparity = %fs", t);
 
-			if(_wordsMaxDepth)
-			{
-				// disparity = baseline * fx / depth;
-				float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth;
-				Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
-			}
-
-			keypoints3D = util3d::generateKeypoints3DDisparity(
-					keypoints,
-					disparity,
-					data.stereoCameraModel());
+			keypoints3D = util3d::generateKeypoints3DStereo(
+					leftCorners,
+					rightCorners,
+					data.stereoCameraModel(),
+					status);
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);

corelib/src/OdometryMono.cpp

@@ -31,8 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d_features.h"
 #include "rtabmap/core/EpipolarGeometry.h"
+#include "rtabmap/core/Stereo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -51,11 +53,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	flowIterations_(Parameters::defaultOdomFlowIterations()),
 	flowEps_(Parameters::defaultOdomFlowEps()),
 	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
-	stereoWinSize_(Parameters::defaultStereoWinSize()),
-	stereoIterations_(Parameters::defaultStereoIterations()),
-	stereoEps_(Parameters::defaultStereoEps()),
-	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
-	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
 	localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
 	initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
 	initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -70,12 +67,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
 	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
 
-	Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
-	Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
-	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
-	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
-	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
-
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
 	Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
@@ -139,11 +130,23 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	{
 		UERROR("Error initializing the memory for Mono Odometry.");
 	}
+
+	bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
+	Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
+	if(stereoOpticalFlow)
+	{
+		stereo_ = new StereoOpticalFlow(parameters);
+	}
+	else
+	{
+		stereo_ = new Stereo(parameters);
+	}
 }
 
 OdometryMono::~OdometryMono()
 {
 	delete memory_;
+	delete stereo_;
 }
 
 void OdometryMono::reset(const Transform & initialPose)
@@ -735,20 +738,21 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							{
 								if(refDepthOrRight_.type() == CV_8UC1)
 								{
-									 newCorners3D = util3d::generateKeypoints3DStereo(
-											 refCorners,
-											 refS->sensorData().imageRaw(),
-											 refDepthOrRight_,
-											 cameraModel.fx(),
-											 data.stereoCameraModel().baseline(),
-											 cameraModel.cx(),
-											 cameraModel.cy(),
-											 Transform::getIdentity(),
-											 stereoWinSize_,
-											 stereoMaxLevel_,
-											 stereoIterations_,
-											 stereoEps_,
-											 stereoMaxSlope_ );
+									StereoCameraModel m = data.stereoCameraModel();
+									m.setLocalTransform(Transform::getIdentity());
+									std::vector<unsigned char> stereoStatus;
+									std::vector<cv::Point2f> rightCorners;
+									rightCorners = stereo_->computeCorrespondences(
+											refS->sensorData().imageRaw(),
+											refDepthOrRight_,
+											refCorners,
+											stereoStatus);
+
+									newCorners3D = util3d::generateKeypoints3DStereo(
+											refCorners,
+											rightCorners,
+											m,
+											stereoStatus);
 								}
 								else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
 								{

corelib/src/OdometryOpticalFlow.cpp

@@ -30,8 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_features.h"
+#include "rtabmap/core/Stereo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -45,33 +47,39 @@ namespace rtabmap {
 
 OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	Odometry(parameters),
+	keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
 	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
 	flowIterations_(Parameters::defaultOdomFlowIterations()),
 	flowEps_(Parameters::defaultOdomFlowEps()),
 	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
-	stereoWinSize_(Parameters::defaultStereoWinSize()),
-	stereoIterations_(Parameters::defaultStereoIterations()),
-	stereoEps_(Parameters::defaultStereoEps()),
-	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
-	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
+	flowGuessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
 	subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
 	subPixIterations_(Parameters::defaultVisSubPixIterations()),
 	subPixEps_(Parameters::defaultVisSubPixEps()),
-	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>),
+	motionSinceLastKeyFrame_(Transform::getIdentity())
 {
+	Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
 	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::kStereoWinSize(), stereoWinSize_);
-	Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
-	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
-	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
-	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
+	Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), flowGuessFromMotion_);
 	Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
 	Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
 	Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
 
+	bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
+	Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
+	if(stereoOpticalFlow)
+	{
+		stereo_ = new StereoOpticalFlow(parameters);
+	}
+	else
+	{
+		stereo_ = new Stereo(parameters);
+	}
+
 	ParametersMap::const_iterator iter;
 	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
 	if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
@@ -106,15 +114,16 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 OdometryOpticalFlow::~OdometryOpticalFlow()
 {
 	delete feature2D_;
+	delete stereo_;
 }
 
-
 void OdometryOpticalFlow::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 	refFrame_ = cv::Mat();
 	refCorners_.clear();
 	refCorners3D_->clear();
+	motionSinceLastKeyFrame_.setIdentity();
 }
 
 // return not null transform if odometry is correctly computed
@@ -168,10 +177,9 @@ Transform OdometryOpticalFlow::computeTransform(
 				uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
 
 		// make guess
-		bool flowGuessByMotion = true;
 		cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
 		Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
-		Transform guess = (this->previousTransform() * localTransform).inverse();
+		Transform guess = (motionSinceLastKeyFrame_*this->previousTransform() * localTransform).inverse();
 		cv::Mat R = (cv::Mat_<double>(3,3) <<
 				(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 				(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -186,7 +194,7 @@ Transform OdometryOpticalFlow::computeTransform(
 			objectPoints[i].y = refCorners3D_->at(i).y;
 			objectPoints[i].z = refCorners3D_->at(i).z;
 		}
-		if(flowGuessByMotion && !this->previousTransform().isIdentity())
+		if(flowGuessFromMotion_ && !(motionSinceLastKeyFrame_*this->previousTransform()).isIdentity())
 		{
 			UDEBUG("project points to new image");
 			cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
@@ -196,7 +204,7 @@ Transform OdometryOpticalFlow::computeTransform(
 		std::vector<unsigned char> status;
 		std::vector<float> err;
 		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-		int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2);
+		int winSize = (newCorners.size()||!flowGuessFromMotion_)?flowWinSize_:(flowWinSize_*2);
 		cv::calcOpticalFlowPyrLK(
 				refFrame_,
 				newLeftFrame,
@@ -205,7 +213,7 @@ Transform OdometryOpticalFlow::computeTransform(
 				status,
 				err,
 				cv::Size(winSize, winSize),
-				(newCorners.size()||!flowGuessByMotion)?flowMaxLevel_:flowMaxLevel_*2,
+				(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:flowMaxLevel_*2,
 				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
 				cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
 		UDEBUG("cv::calcOpticalFlowPyrLK() end");
@@ -246,20 +254,35 @@ Transform OdometryOpticalFlow::computeTransform(
 				if(!data.rightRaw().empty())
 				{
 					// stereo
-					newCorners3DKept = util3d::generateKeypoints3DStereo(
-							newCornersKept,
+					std::vector<unsigned char> stereoStatus;
+					std::vector<cv::Point2f> rightCorners;
+					rightCorners = stereo_->computeCorrespondences(
 							newLeftFrame,
 							data.rightRaw(),
-							data.stereoCameraModel().left().fx(),
-							data.stereoCameraModel().baseline(),
-							data.stereoCameraModel().left().cx(),
-							data.stereoCameraModel().left().cy(),
-							data.stereoCameraModel().left().localTransform(),
-							stereoWinSize_,
-							stereoMaxLevel_,
-							stereoIterations_,
-							stereoEps_,
-							stereoMaxSlope_);
+							newCornersKept,
+							stereoStatus);
+
+					if(this->getMaxDepth() > 0.0f)
+					{
+						UASSERT(status.size() == newCornersKept.size() && status.size() == rightCorners.size());
+						for(unsigned int i=0; i<status.size(); ++i)
+						{
+							if(status[i] != 0)
+							{
+								float d = data.stereoCameraModel().computeDepth(newCornersKept[i].x - rightCorners[i].x);
+								if(this->getMaxDepth() > 0.0f && d > this->getMaxDepth())
+								{
+									status[i] = 0;
+								}
+							}
+						}
+					}
+
+					newCorners3DKept = util3d::generateKeypoints3DStereo(
+							newCornersKept,
+							rightCorners,
+							data.stereoCameraModel(),
+							stereoStatus);
 				}
 				else
 				{
@@ -433,120 +456,133 @@ Transform OdometryOpticalFlow::computeTransform(
 	newCorners.clear();
 	if(!output.isNull())
 	{
-		// Copy or generate new keypoints
-		if(data.keypoints().size())
-		{
-			cv::KeyPoint::convert(data.keypoints(), newCorners);
-		}
-		else
-		{
-			// generate kpts
-			std::vector<cv::KeyPoint> newKtps;
-			cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
-			newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
+		output = motionSinceLastKeyFrame_.inverse() * output;
 
-			if(newKtps.size())
+		// new key-frame?
+		if(keyFrameThr_ <= 0 || inliers <= keyFrameThr_)
+		{
+			// Copy or generate new keypoints
+			if(data.keypoints().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((int)newCorners.size() >= this->getMinInliers())
-		{
-			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;
-			UTimer corner3dTimer;
-			if(!data.rightRaw().empty())
-			{
-				/// stereo
-				pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
-						newCorners,
-						newLeftFrame,
-						data.rightRaw(),
-						data.stereoCameraModel().left().fx(),
-						data.stereoCameraModel().baseline(),
-						data.stereoCameraModel().left().cx(),
-						data.stereoCameraModel().left().cy(),
-						Transform::getIdentity(),
-						stereoWinSize_,
-						stereoMaxLevel_,
-						stereoIterations_,
-						stereoEps_,
-						stereoMaxSlope_);
-				UASSERT(refCorners3DTmp->size() == newCorners.size());
-				for(unsigned int i=0; i<newCorners.size(); ++i)
-				{
-					if(pcl::isFinite(refCorners3DTmp->at(i)) &&
-						(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
-					{
-						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform());
-						newCornersFiltered[oi] = newCorners[i];
-						++oi;
-					}
-				}
+				cv::KeyPoint::convert(data.keypoints(), newCorners);
 			}
 			else
 			{
-				// depth
-				for(unsigned int i=0; i<newCorners.size(); ++i)
+				// generate kpts
+				std::vector<cv::KeyPoint> newKtps;
+				cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
+				newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
+
+				if(newKtps.size())
 				{
-					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
-					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
+					cv::KeyPoint::convert(newKtps, newCorners);
+
+					if(subPixWinSize_ > 0 && subPixIterations_ > 0)
 					{
-						pcl::PointXYZ pt = util3d::projectDepthTo3D(
-								data.depthRaw(),
-								newCorners[i].x,
-								newCorners[i].y,
-								data.cameraModels()[0].cx(),
-								data.cameraModels()[0].cy(),
-								data.cameraModels()[0].fx(),
-								data.cameraModels()[0].fy(),
-								true);
-						if(pcl::isFinite(pt) &&
-							pt.z > 0 &&
-							(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
+						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((int)newCorners.size() >= this->getMinInliers())
+			{
+				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;
+				UTimer corner3dTimer;
+				if(!data.rightRaw().empty())
+				{
+					// stereo
+					std::vector<unsigned char> stereoStatus;
+					std::vector<cv::Point2f> rightCorners;
+					rightCorners = stereo_->computeCorrespondences(
+							newLeftFrame,
+							data.rightRaw(),
+							newCorners,
+							stereoStatus);
+
+					pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
+							newCorners,
+							rightCorners,
+							data.stereoCameraModel(),
+							stereoStatus);
+
+					UASSERT(refCorners3DTmp->size() == newCorners.size());
+					for(unsigned int i=0; i<newCorners.size(); ++i)
+					{
+						if(pcl::isFinite(refCorners3DTmp->at(i)) &&
+						  (this->getMaxDepth() <= 0.0f || data.stereoCameraModel().computeDepth(newCorners[i].x - rightCorners[i].x) <= this->getMaxDepth() ))
 						{
-							newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
+							newCorners3D->at(oi) = refCorners3DTmp->at(i);
 							newCornersFiltered[oi] = newCorners[i];
 							++oi;
 						}
 					}
 				}
-			}
-			UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
-			newCornersFiltered.resize(oi);
-			newCorners3D->resize(oi);
+				else
+				{
+					// depth
+					for(unsigned int i=0; i<newCorners.size(); ++i)
+					{
+						if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
+						   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
+						{
+							pcl::PointXYZ pt = util3d::projectDepthTo3D(
+									data.depthRaw(),
+									newCorners[i].x,
+									newCorners[i].y,
+									data.cameraModels()[0].cx(),
+									data.cameraModels()[0].cy(),
+									data.cameraModels()[0].fx(),
+									data.cameraModels()[0].fy(),
+									true);
+							if(pcl::isFinite(pt) &&
+								pt.z > 0 &&
+								(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
+							{
+								newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
+								newCornersFiltered[oi] = newCorners[i];
+								++oi;
+							}
+						}
+					}
+				}
+				UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
+				newCornersFiltered.resize(oi);
+				newCorners3D->resize(oi);
 
-			if((int)newCornersFiltered.size() >= this->getMinInliers())
-			{
-				refFrame_ = newLeftFrame;
-				refCorners_ = newCornersFiltered;
-				refCorners3D_ = newCorners3D;
+				if((int)newCornersFiltered.size() >= this->getMinInliers())
+				{
+					refFrame_ = newLeftFrame;
+					refCorners_ = newCornersFiltered;
+					refCorners3D_ = newCorners3D;
+
+					//reset motion
+					motionSinceLastKeyFrame_.setIdentity();
+				}
+				else
+				{
+					UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
+							(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
+					output.setNull();
+				}
 			}
 			else
 			{
-				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
-						(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
+				UWARN("Too low 2D corners (%d), ignoring new frame...",
+						(int)newCorners.size());
 				output.setNull();
 			}
 		}
 		else
 		{
-			UWARN("Too low 2D corners (%d), ignoring new frame...",
-					(int)newCorners.size());
-			output.setNull();
+			motionSinceLastKeyFrame_ *= output;
 		}
 	}
 
@@ -555,7 +591,7 @@ Transform OdometryOpticalFlow::computeTransform(
 		info->type = 1;
 		info->variance = variance;
 		info->inliers = inliers;
-		info->features = (int)newCorners.size();
+		info->features = (int)refCorners_.size();
 		info->matches = correspondences;
 	}
 

corelib/src/Parameters.cpp

@@ -118,6 +118,21 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo)
 	return odomParameters;
 }
 
+ParametersMap Parameters::getDefaultParameters(const std::string & group)
+{
+	rtabmap::ParametersMap parameters;
+	const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::const_iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
+	{
+		if(iter->first.compare(group) == 0)
+		{
+			parameters.insert(*iter);
+		}
+	}
+	UASSERT_MSG(parameters.size(), uFormat("No parameters found for group %s!", group.c_str()).c_str());
+	return parameters;
+}
+
 const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemovedParameters()
 {
 	if(removedParameters_.empty())
@@ -201,6 +216,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D",              std::make_pair(true,  Parameters::kOptimizerSlam2D())));
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeVarianceIgnored",     std::make_pair(true,  Parameters::kOptimizerVarianceIgnored())));
 
+		removedParameters_.insert(std::make_pair("Stereo/WinSize",                   std::make_pair(true,  Parameters::kStereoWinWidth())));
+
 		// before 0.11.0
 		removedParameters_.insert(std::make_pair("GFTT/MaxCorners",                  std::make_pair(true, Parameters::kVisMaxFeatures())));
 		removedParameters_.insert(std::make_pair("LccBow/MaxDepth",                  std::make_pair(true, Parameters::kVisMaxDepth())));
@@ -270,7 +287,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atoi(iter->second.c_str());
+		value = uStr2Int(iter->second.c_str());
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
@@ -278,7 +295,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atoi(iter->second.c_str());
+		value = uStr2Int(iter->second.c_str());
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
@@ -305,6 +322,17 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 		value = iter->second;
 	}
 }
+void Parameters::parse(const ParametersMap & parameters, ParametersMap & parametersOut)
+{
+	for(ParametersMap::iterator iter=parametersOut.begin(); iter!=parametersOut.end(); ++iter)
+	{
+		ParametersMap::const_iterator jter = parameters.find(iter->first);
+		if(jter != parameters.end())
+		{
+			iter->second = jter->second;
+		}
+	}
+}
 
 
 void Parameters::readINI(const std::string & configFile, ParametersMap & parameters)

corelib/src/Stereo.cpp

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Stereo.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <opencv2/video/tracking.hpp>
+
+namespace rtabmap {
+
+Stereo::Stereo(const ParametersMap & parameters) :
+		winWidth_(Parameters::defaultStereoWinWidth()),
+		winHeight_(Parameters::defaultStereoWinHeight()),
+		iterations_(Parameters::defaultStereoIterations()),
+		maxLevel_(Parameters::defaultStereoMaxLevel()),
+		minDisparity_(Parameters::defaultStereoMinDisparity()),
+		maxDisparity_(Parameters::defaultStereoMaxDisparity()),
+		winSSD_(Parameters::defaultStereoSSD())
+{
+}
+
+void Stereo::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kStereoWinWidth(), winWidth_);
+	Parameters::parse(parameters, Parameters::kStereoWinHeight(), winHeight_);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), iterations_);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), maxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoMinDisparity(), minDisparity_);
+	Parameters::parse(parameters, Parameters::kStereoMaxDisparity(), maxDisparity_);
+	Parameters::parse(parameters, Parameters::kStereoSSD(), winSSD_);
+}
+
+std::vector<cv::Point2f> Stereo::computeCorrespondences(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("util2d::calcStereoCorrespondences() begin");
+	rightCorners = util2d::calcStereoCorrespondences(
+					leftImage,
+					rightImage,
+					leftCorners,
+					status,
+					cv::Size(winWidth_, winHeight_),
+					maxLevel_,
+					iterations_,
+					minDisparity_,
+					maxDisparity_,
+					winSSD_);
+	UDEBUG("util2d::calcStereoCorrespondences() end");
+	return rightCorners;
+}
+
+StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
+		Stereo(parameters),
+		epsilon_(Parameters::defaultStereoEps()),
+		maxSlope_(Parameters::defaultStereoMaxSlope())
+{
+	this->parseParameters(parameters);
+}
+
+void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
+{
+	Stereo::parseParameters(parameters);
+	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), maxSlope_);
+}
+
+
+std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
+	std::vector<float> err;
+	util2d::calcOpticalFlowPyrLKStereo(
+			leftImage,
+			rightImage,
+			leftCorners,
+			rightCorners,
+			status,
+			err,
+			this->winSize(),
+			this->maxLevel(),
+			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
+			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
+	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
+	for(unsigned int i=0; i<status.size(); ++i)
+	{
+		if(status[i]!=0)
+		{
+			float disparity = leftCorners[i].x - rightCorners[i].x;
+			float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
+			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()) ||
+			   (maxSlope_ > 0.0f && fabs(leftCorners[i].y-rightCorners[i].y) > 1.0f && slope > maxSlope_))
+			{
+				status[i] = 0;
+			}
+		}
+	}
+	return rightCorners;
+}
+
+} /* namespace rtabmap */

corelib/src/StereoCameraModel.cpp

@@ -0,0 +1,214 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace rtabmap {
+
+void StereoCameraModel::setName(const std::string & name)
+{
+	name_=name;
+	left_.setName(name_+"_left");
+	right_.setName(name_+"_right");
+}
+
+bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
+{
+	name_ = cameraName;
+	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
+	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
+		//load rotation, translation
+		R_ = cv::Mat();
+		T_ = cv::Mat();
+
+		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		if(UFile::exists(filePath))
+		{
+			UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::READ);
+
+			name_ = (int)fs["camera_name"];
+
+			// import from ROS calibration format
+			cv::FileNode n = fs["rotation_matrix"];
+			int rows = (int)n["rows"];
+			int cols = (int)n["cols"];
+			std::vector<double> data;
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["translation_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 1);
+			T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["essential_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["fundamental_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			fs.release();
+
+			return true;
+		}
+		else
+		{
+			UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
+		}
+	}
+	return false;
+}
+bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
+{
+	if(left_.save(directory) && right_.save(directory))
+	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
+		std::string filePath = directory+"/"+name_+"_pose.yaml";
+		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
+		{
+			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
+
+			// export in ROS calibration format
+
+			fs << "camera_name" << name_;
+
+			fs << "rotation_matrix" << "{";
+			fs << "rows" << R_.rows;
+			fs << "cols" << R_.cols;
+			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+			fs << "}";
+
+			fs << "translation_matrix" << "{";
+			fs << "rows" << T_.rows;
+			fs << "cols" << T_.cols;
+			fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
+			fs << "}";
+
+			fs << "essential_matrix" << "{";
+			fs << "rows" << E_.rows;
+			fs << "cols" << E_.cols;
+			fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
+			fs << "}";
+
+			fs << "fundamental_matrix" << "{";
+			fs << "rows" << F_.rows;
+			fs << "cols" << F_.cols;
+			fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
+			fs << "}";
+
+			fs.release();
+
+			return true;
+		}
+	}
+	return false;
+}
+
+void StereoCameraModel::scale(double scale)
+{
+	left_.scale(scale);
+	right_.scale(scale);
+}
+
+float StereoCameraModel::computeDepth(float disparity) const
+{
+	//depth = baseline * f / (disparity + cx1-cx0);
+	UASSERT(this->isValid());
+	if(disparity == 0.0f)
+	{
+		return 0.0f;
+	}
+	return baseline() * left().fx() / (disparity + right().cx() - left().cx());
+}
+
+float StereoCameraModel::computeDisparity(float depth) const
+{
+	// disparity = (baseline * fx / depth) - (cx1-cx0);
+	UASSERT(this->isValid());
+	if(depth == 0.0f)
+	{
+		return 0.0f;
+	}
+	return baseline() * left().fx() / depth - right().cx() + left().cx();
+}
+
+float StereoCameraModel::computeDisparity(unsigned short depth) const
+{
+	// disparity = (baseline * fx / depth) - (cx1-cx0);
+	UASSERT(this->isValid());
+	if(depth == 0)
+	{
+		return 0.0f;
+	}
+	return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
+}
+
+Transform StereoCameraModel::stereoTransform() const
+{
+	if(!R_.empty() && !T_.empty())
+	{
+		return Transform(
+				R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
+				R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
+				R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
+	}
+	return Transform();
+}
+
+} /* namespace rtabmap */

corelib/src/StereoDense.cpp

@@ -0,0 +1,122 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/StereoDense.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace rtabmap {
+
+StereoBM::StereoBM(int blockSize, int numDisparities) :
+		blockSize_(blockSize),
+		minDisparity_(Parameters::defaultStereoBMMinDisparity()),
+		numDisparities_(numDisparities),
+		preFilterSize_(Parameters::defaultStereoBMPreFilterSize()),
+		preFilterCap_(Parameters::defaultStereoBMPreFilterCap()),
+		uniquenessRatio_(Parameters::defaultStereoBMUniquenessRatio()),
+		textureThreshold_(Parameters::defaultStereoBMTextureThreshold()),
+		speckleWindowSize_(Parameters::defaultStereoBMSpeckleWindowSize()),
+		speckleRange_(Parameters::defaultStereoBMSpeckleRange())
+{
+}
+StereoBM::StereoBM(const ParametersMap & parameters) :
+		StereoDense(parameters),
+		blockSize_(Parameters::defaultStereoBMBlockSize()),
+		minDisparity_(Parameters::defaultStereoBMMinDisparity()),
+		numDisparities_(Parameters::defaultStereoBMNumDisparities()),
+		preFilterSize_(Parameters::defaultStereoBMPreFilterSize()),
+		preFilterCap_(Parameters::defaultStereoBMPreFilterCap()),
+		uniquenessRatio_(Parameters::defaultStereoBMUniquenessRatio()),
+		textureThreshold_(Parameters::defaultStereoBMTextureThreshold()),
+		speckleWindowSize_(Parameters::defaultStereoBMSpeckleWindowSize()),
+		speckleRange_(Parameters::defaultStereoBMSpeckleRange())
+{
+	this->parseParameters(parameters);
+}
+
+void StereoBM::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kStereoBMBlockSize(), blockSize_);
+	Parameters::parse(parameters, Parameters::kStereoBMMinDisparity(), minDisparity_);
+	Parameters::parse(parameters, Parameters::kStereoBMNumDisparities(), numDisparities_);
+	Parameters::parse(parameters, Parameters::kStereoBMPreFilterSize(), preFilterSize_);
+	Parameters::parse(parameters, Parameters::kStereoBMPreFilterCap(), preFilterCap_);
+	Parameters::parse(parameters, Parameters::kStereoBMUniquenessRatio(), uniquenessRatio_);
+	Parameters::parse(parameters, Parameters::kStereoBMTextureThreshold(), textureThreshold_);
+	Parameters::parse(parameters, Parameters::kStereoBMPreFilterSize(), speckleWindowSize_);
+	Parameters::parse(parameters, Parameters::kStereoBMSpeckleRange(), speckleRange_);
+}
+
+cv::Mat StereoBM::computeDisparity(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage) const
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty());
+	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
+	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
+
+	cv::Mat leftMono;
+	if(leftImage.channels() == 3)
+	{
+		cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		leftMono = leftImage;
+	}
+
+	cv::Mat disparity;
+#if CV_MAJOR_VERSION < 3
+	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
+	stereo.state->SADWindowSize = blockSize_;
+	stereo.state->minDisparity = minDisparity_;
+	stereo.state->numberOfDisparities = numDisparities_;
+	stereo.state->preFilterSize = preFilterSize_;
+	stereo.state->preFilterCap = preFilterCap_;
+	stereo.state->uniquenessRatio = uniquenessRatio_;
+	stereo.state->textureThreshold = textureThreshold_;
+	stereo.state->speckleWindowSize = speckleWindowSize_;
+	stereo.state->speckleRange = speckleRange_;
+	stereo(leftMono, rightImage, disparity, CV_16SC1);
+#else
+	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
+	stereo->setBlockSize(blockSize_);
+	stereo->setMinDisparity(minDisparity_);
+	stereo->setNumDisparities(numDisparities_);
+	stereo->setPreFilterSize(preFilterSize_);
+	stereo->setPreFilterCap(preFilterCap_);
+	stereo->setUniquenessRatio(uniquenessRatio_);
+	stereo->setTextureThreshold(textureThreshold_);
+	stereo->setSpeckleWindowSize(speckleWindowSize_);
+	stereo->setSpeckleRange(speckleRange_);
+	stereo->compute(leftMono, rightImage, disparity);
+#endif
+	return disparity;
+}
+
+} /* namespace rtabmap */

corelib/src/util2d.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2014, cv::Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -8,7 +8,7 @@ modification, are permitted provided that the following conditions are met:
       notice, this list of conditions and the following disclaimer.
     * Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
+      documentation and/or other cv::Materials provided with the distribution.
     * Neither the name of the Universite de Sherbrooke nor the
       names of its contributors may be used to endorse or promote products
       derived from this software without specific prior written permission.
@@ -29,9 +29,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <map>
 
 namespace rtabmap
 {
@@ -39,6 +44,675 @@ namespace rtabmap
 namespace util2d
 {
 
+// SSD: Sum of Squared Differences
+float ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight)
+{
+	UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
+	UASSERT(windowLeft.type() == windowRight.type());
+	UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
+	UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
+
+	float score = 0.0f;
+	for(int v=0; v<windowLeft.rows; ++v)
+	{
+		for(int u=0; u<windowLeft.cols; ++u)
+		{
+			float s = 0.0f;
+			if(windowLeft.type() == CV_8UC1)
+			{
+				s = float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u));
+			}
+			else if(windowLeft.type() == CV_32FC1)
+			{
+				s = windowLeft.at<float>(v,u)-windowRight.at<float>(v,u);
+			}
+			else if(windowLeft.type() == CV_16SC2)
+			{
+				float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
+				float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
+				s = sL - sR;
+			}
+
+			score += s*s;
+		}
+	}
+	return score;
+}
+
+// SAD: Sum of Absolute intensity Differences
+float sad(const cv::Mat & windowLeft, const cv::Mat & windowRight)
+{
+	UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
+	UASSERT(windowLeft.type() == windowRight.type());
+	UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
+	UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
+
+	float score = 0.0f;
+	for(int v=0; v<windowLeft.rows; ++v)
+	{
+		for(int u=0; u<windowLeft.cols; ++u)
+		{
+			if(windowLeft.type() == CV_8UC1)
+			{
+				score += fabs(float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u)));
+			}
+			else if(windowLeft.type() == CV_32FC1)
+			{
+				score += fabs(windowLeft.at<float>(v,u)-windowRight.at<float>(v,u));
+			}
+			else if(windowLeft.type() == CV_16SC2)
+			{
+				float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
+				float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
+				score += fabs(sL - sR);
+			}
+		}
+	}
+	return score;
+}
+
+std::vector<cv::Point2f> calcStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status,
+		cv::Size winSize,
+		int maxLevel,
+		int iterations,
+		int minDisparity,
+		int maxDisparity,
+		bool ssdApproach)
+{
+	UDEBUG("winSize=(%d,%d)", winSize.width, winSize.height);
+	UDEBUG("maxLevel=%d", maxLevel);
+	UDEBUG("minDisparity=%d", minDisparity);
+	UDEBUG("maxDisparity=%d", maxDisparity);
+	UDEBUG("iterations=%d", iterations);
+	UDEBUG("ssdApproach=%d", ssdApproach?1:0);
+
+	// window should be odd
+	if(winSize.width%2 == 0)
+	{
+		winSize.width+=1;
+	}
+	if(winSize.height%2 == 0)
+	{
+		winSize.height+=1;
+	}
+
+	cv::Size halfWin((winSize.width-1)/2, (winSize.height-1)/2);
+
+	UTimer timer;
+	double pyramidTime = 0.0;
+	double disparityTime = 0.0;
+	double subpixelTime = 0.0;
+
+	std::vector<cv::Point2f> rightCorners(leftCorners.size());
+	std::vector<cv::Mat> leftPyramid, rightPyramid;
+	maxLevel =  cv::buildOpticalFlowPyramid( leftImage, leftPyramid, winSize, maxLevel, false);
+	maxLevel =  cv::buildOpticalFlowPyramid( rightImage, rightPyramid, winSize, maxLevel, false);
+	pyramidTime = timer.ticks();
+
+	status = std::vector<unsigned char>(leftCorners.size(), 0);
+	int totalIterations = 0;
+	int noSubPixel = 0;
+	for(unsigned int i=0; i<leftCorners.size(); ++i)
+	{
+		int oi=0;
+		float bestScore = -1.0f;
+		float secondBest = -1.0f;
+		int bestScoreIndex = -1;
+		int tmpMinDisparity = minDisparity;
+		int tmpMaxDisparity = maxDisparity;
+
+		int iterations = 0;
+		std::vector<float> scores;
+		for(int level=maxLevel; level>=0; --level)
+		{
+			UASSERT(level < (int)leftPyramid.size());
+
+			cv::Point2i center(int(leftCorners[i].x/float(1<<level)), int(leftCorners[i].y/float(1<<level)));
+
+			oi=0;
+			bestScore = -1.0f;
+			secondBest = -1.0f;
+			bestScoreIndex = -1;
+			int localMaxDisparity = -tmpMaxDisparity / (1<<level);
+			int localMinDisparity = -tmpMinDisparity / (1<<level);
+
+			if(center.x-halfWin.width-(level==0?1:0) >=0 && center.x+halfWin.width+(level==0?1:0) < leftPyramid[level].cols &&
+			   center.y-halfWin.height >=0 && center.y+halfWin.height < leftPyramid[level].rows)
+			{
+				cv::Mat windowLeft(leftPyramid[level],
+						cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
+						cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
+				int minCol = center.x+localMaxDisparity-halfWin.width-1;
+				if(minCol < 0)
+				{
+					localMaxDisparity -= minCol;
+				}
+
+				int maxCol = center.x+localMinDisparity+halfWin.width+1;
+				if(maxCol >= leftPyramid[level].cols)
+				{
+					localMinDisparity += maxCol-leftPyramid[level].cols-1;
+				}
+
+				scores = std::vector<float>(localMinDisparity-localMaxDisparity+1, 0.0f);
+				for(int d=localMinDisparity; d>localMaxDisparity; --d)
+				{
+					++iterations;
+					cv::Mat windowRight(rightPyramid[level],
+									cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
+									cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
+					scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
+					if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
+					{
+						secondBest = bestScore;
+						bestScoreIndex = oi;
+						bestScore = scores[oi];
+					}
+					++oi;
+				}
+
+				if(bestScoreIndex>=0)
+				{
+					if(level>0)
+					{
+						tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
+						tmpMaxDisparity+=tmpMaxDisparity%level;
+						if(tmpMaxDisparity > maxDisparity)
+						{
+							tmpMaxDisparity = maxDisparity;
+						}
+						tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
+						tmpMinDisparity -= tmpMinDisparity%level;
+						if(tmpMinDisparity < minDisparity)
+						{
+							tmpMinDisparity = minDisparity;
+						}
+					}
+				}
+			}
+		}
+		disparityTime+=timer.ticks();
+		totalIterations+=iterations;
+
+		if(bestScoreIndex>=0)
+		{
+			//subpixel refining
+			int d = -(tmpMinDisparity+bestScoreIndex);
+
+			cv::Mat windowLeft(winSize, CV_32FC1);
+			cv::Mat windowRight(winSize, CV_32FC1);
+			cv::getRectSubPix(leftPyramid[0],
+					winSize,
+					leftCorners[i],
+					windowLeft,
+					windowLeft.type());
+			if(leftCorners[i].x != float(int(leftCorners[i].x)))
+			{
+				//recompute bestScore if the pt is not integer
+				cv::getRectSubPix(rightPyramid[0],
+						winSize,
+						cv::Point2f(leftCorners[i].x+float(d), leftCorners[i].y),
+						windowRight,
+						windowRight.type());
+				bestScore = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
+			}
+
+			float xc = leftCorners[i].x+float(d);
+			float vc = bestScore;
+			float step = 0.5f;
+			std::map<float, float> cache;
+			bool reject = false;
+			for(int it=0; it<iterations; ++it)
+			{
+				float x1 = xc-step;
+				float x2 = xc+step;
+				float v1 = uValue(cache, x1, 0.0f);
+				float v2 = uValue(cache, x2, 0.0f);
+				if(v1 == 0.0f)
+				{
+					cv::getRectSubPix(rightPyramid[0],
+							winSize,
+							cv::Point2f(x1, leftCorners[i].y),
+							windowRight,
+							windowRight.type());
+					v1 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
+				}
+				if(v2 == 0.0f)
+				{
+					cv::getRectSubPix(rightPyramid[0],
+							winSize,
+							cv::Point2f(x2, leftCorners[i].y),
+							windowRight,
+							windowRight.type());
+					v2 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
+				}
+
+				float previousXc = xc;
+				float previousVc = vc;
+
+				xc = v1<vc&&v1<v2?x1:v2<vc&&v2<v1?x2:xc;
+				vc = v1<vc&&v1<v2?v1:v2<vc&&v2<v1?v2:vc;
+
+				if(previousXc == xc)
+				{
+					step /= 2.0f;
+				}
+				else
+				{
+					cache.insert(std::make_pair(previousXc, previousVc));
+				}
+
+				if(xc < leftCorners[i].x+float(d)-1.0f || xc > leftCorners[i].x+float(d)+1.0f)
+				{
+					reject = true;
+					break;
+				}
+			}
+
+			if(leftCorners[i].x+float(d) == xc)
+			{
+				++noSubPixel;
+			}
+
+			rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
+			status[i] = reject?0:1;
+		}
+		subpixelTime+=timer.ticks();
+	}
+	UDEBUG("noSubPixel=%d", noSubPixel);
+	UDEBUG("totalIterations=%d", totalIterations);
+	UDEBUG("Time pyramid = %f s", pyramidTime);
+	UDEBUG("Time disparity = %f s", disparityTime);
+	UDEBUG("Time sub-pixel = %f s", subpixelTime);
+
+	return rightCorners;
+}
+
+typedef float acctype;
+typedef float itemtype;
+#define  CV_DESCALE(x,n)     (((x) + (1 << ((n)-1))) >> (n))
+
+//
+// Adapted from OpenCV cv::calcOpticalFlowPyrLK() to force
+// only optical flow on x-axis (assuming that prevImg is the left
+// image and nextImg is the right image):
+// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L1088
+//
+// The difference is on this line:
+// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L683-L684
+// - cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), (float)((A12*b1 - A11*b2) * D));
+// + cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), 0); //<--- note the 0 for y
+//
+void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
+                           cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
+                           cv::OutputArray _status, cv::OutputArray _err,
+                           cv::Size winSize, int maxLevel,
+                           cv::TermCriteria criteria,
+                           int flags, double minEigThreshold )
+{
+    cv::Mat prevPtsMat = _prevPts.getMat();
+    const int derivDepth = cv::DataType<short>::depth;
+
+    CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
+
+    int level=0, i, npoints;
+    CV_Assert( (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 );
+
+    if( npoints == 0 )
+    {
+        _nextPts.release();
+        _status.release();
+        _err.release();
+        return;
+    }
+
+    if( !(flags & cv::OPTFLOW_USE_INITIAL_FLOW) )
+        _nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
+
+    cv::Mat nextPtsMat = _nextPts.getMat();
+    CV_Assert( nextPtsMat.checkVector(2, CV_32F, true) == npoints );
+
+    const cv::Point2f* prevPts = prevPtsMat.ptr<cv::Point2f>();
+    cv::Point2f* nextPts = nextPtsMat.ptr<cv::Point2f>();
+
+    _status.create((int)npoints, 1, CV_8U, -1, true);
+    cv::Mat statusMat = _status.getMat(), errMat;
+    CV_Assert( statusMat.isContinuous() );
+    uchar* status = statusMat.ptr();
+    float* err = 0;
+
+    for( i = 0; i < npoints; i++ )
+        status[i] = true;
+
+    if( _err.needed() )
+    {
+        _err.create((int)npoints, 1, CV_32F, -1, true);
+        errMat = _err.getMat();
+        CV_Assert( errMat.isContinuous() );
+        err = errMat.ptr<float>();
+    }
+
+    std::vector<cv::Mat> prevPyr, nextPyr;
+    int levels1 = -1;
+    int lvlStep1 = 1;
+    int levels2 = -1;
+    int lvlStep2 = 1;
+
+
+    if(_prevImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
+	{
+		//create pyramid
+		maxLevel =  cv::buildOpticalFlowPyramid(_prevImg, prevPyr, winSize, maxLevel, true);
+	}
+	else if(_prevImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
+	{
+		_prevImg.getMatVector(prevPyr);
+	}
+
+	levels1 = int(prevPyr.size()) - 1;
+	CV_Assert(levels1 >= 0);
+
+	if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
+	{
+		lvlStep1 = 2;
+		levels1 /= 2;
+	}
+
+	// ensure that pyramid has required padding
+	if(levels1 > 0)
+	{
+		cv::Size fullSize;
+		cv::Point ofs;
+		prevPyr[lvlStep1].locateROI(fullSize, ofs);
+		CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
+			&& ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
+			&& ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
+	}
+
+	if(levels1 < maxLevel)
+		maxLevel = levels1;
+
+	if(_nextImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
+	{
+		//create pyramid
+		maxLevel =  cv::buildOpticalFlowPyramid(_nextImg, nextPyr, winSize, maxLevel, false);
+	}
+	else if(_nextImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
+    {
+        _nextImg.getMatVector(nextPyr);
+    }
+
+	levels2 = int(nextPyr.size()) - 1;
+	CV_Assert(levels2 >= 0);
+
+	if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
+	{
+		lvlStep2 = 2;
+		levels2 /= 2;
+	}
+
+	// ensure that pyramid has required padding
+	if(levels2 > 0)
+	{
+		cv::Size fullSize;
+		cv::Point ofs;
+		nextPyr[lvlStep2].locateROI(fullSize, ofs);
+		CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
+			&& ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
+			&& ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
+	}
+
+	if(levels2 < maxLevel)
+		maxLevel = levels2;
+
+    if( (criteria.type & cv::TermCriteria::COUNT) == 0 )
+        criteria.maxCount = 30;
+    else
+        criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
+    if( (criteria.type & cv::TermCriteria::EPS) == 0 )
+        criteria.epsilon = 0.01;
+    else
+        criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
+    criteria.epsilon *= criteria.epsilon;
+
+    // for all pyramids
+    for( level = maxLevel; level >= 0; level-- )
+    {
+        cv::Mat derivI = prevPyr[level * lvlStep1 + 1];
+
+        CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
+        CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
+
+        const cv::Mat & prevImg = prevPyr[level * lvlStep1];
+        const cv::Mat & prevDeriv = derivI;
+        const cv::Mat & nextImg = nextPyr[level * lvlStep2];
+
+        // for all corners
+        {
+        	cv::Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
+			const cv::Mat& I = prevImg;
+			const cv::Mat& J = nextImg;
+			const cv::Mat& derivI = prevDeriv;
+
+			int j, cn = I.channels(), cn2 = cn*2;
+			cv::AutoBuffer<short> _buf(winSize.area()*(cn + cn2));
+			int derivDepth = cv::DataType<short>::depth;
+
+			cv::Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (short*)_buf);
+			cv::Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (short*)_buf + winSize.area()*cn);
+
+			for( int ptidx = 0; ptidx < npoints; ptidx++ )
+			{
+				cv::Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
+				cv::Point2f nextPt;
+				if( level == maxLevel )
+				{
+					if( flags & cv::OPTFLOW_USE_INITIAL_FLOW )
+						nextPt = nextPts[ptidx]*(float)(1./(1 << level));
+					else
+						nextPt = prevPt;
+				}
+				else
+					nextPt = nextPts[ptidx]*2.f;
+				nextPts[ptidx] = nextPt;
+
+				cv::Point2i iprevPt, inextPt;
+				prevPt -= halfWin;
+				iprevPt.x = cvFloor(prevPt.x);
+				iprevPt.y = cvFloor(prevPt.y);
+
+				if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
+					iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
+				{
+					if( level == 0 )
+					{
+						if( status )
+							status[ptidx] = false;
+						if( err )
+							err[ptidx] = 0;
+					}
+					continue;
+				}
+
+				float a = prevPt.x - iprevPt.x;
+				float b = prevPt.y - iprevPt.y;
+				const int W_BITS = 14, W_BITS1 = 14;
+				const float FLT_SCALE = 1.f/(1 << 20);
+				int iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
+				int iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
+				int iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
+				int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
+
+				int dstep = (int)(derivI.step/derivI.elemSize1());
+				int stepI = (int)(I.step/I.elemSize1());
+				int stepJ = (int)(J.step/J.elemSize1());
+				acctype iA11 = 0, iA12 = 0, iA22 = 0;
+				float A11, A12, A22;
+
+				// extract the patch from the first image, compute covariation cv::Matrix of derivatives
+				int x, y;
+				for( y = 0; y < winSize.height; y++ )
+				{
+					const uchar* src = I.ptr() + (y + iprevPt.y)*stepI + iprevPt.x*cn;
+					const short* dsrc = derivI.ptr<short>() + (y + iprevPt.y)*dstep + iprevPt.x*cn2;
+
+					short* Iptr = IWinBuf.ptr<short>(y);
+					short* dIptr = derivIWinBuf.ptr<short>(y);
+
+					x = 0;
+
+					for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
+					{
+						int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
+											  src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
+						int ixval = CV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
+											   dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
+						int iyval = CV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
+											   dsrc[dstep+cn2+1]*iw11, W_BITS1);
+
+						Iptr[x] = (short)ival;
+						dIptr[0] = (short)ixval;
+						dIptr[1] = (short)iyval;
+
+						iA11 += (itemtype)(ixval*ixval);
+						iA12 += (itemtype)(ixval*iyval);
+						iA22 += (itemtype)(iyval*iyval);
+					}
+				}
+
+				A11 = iA11*FLT_SCALE;
+				A12 = iA12*FLT_SCALE;
+				A22 = iA22*FLT_SCALE;
+
+				float D = A11*A22 - A12*A12;
+				float minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
+								4.f*A12*A12))/(2*winSize.width*winSize.height);
+
+				if( err && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) != 0 )
+					err[ptidx] = (float)minEig;
+
+				if( minEig < minEigThreshold || D < FLT_EPSILON )
+				{
+					if( level == 0 && status )
+						status[ptidx] = false;
+					continue;
+				}
+
+				D = 1.f/D;
+
+				nextPt -= halfWin;
+				cv::Point2f prevDelta;
+
+				for( j = 0; j < criteria.maxCount; j++ )
+				{
+					inextPt.x = cvFloor(nextPt.x);
+					inextPt.y = cvFloor(nextPt.y);
+
+					if( inextPt.x < -winSize.width || inextPt.x >= J.cols ||
+					   inextPt.y < -winSize.height || inextPt.y >= J.rows )
+					{
+						if( level == 0 && status )
+							status[ptidx] = false;
+						break;
+					}
+
+					a = nextPt.x - inextPt.x;
+					b = nextPt.y - inextPt.y;
+					iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
+					iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
+					iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
+					iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
+					acctype ib1 = 0, ib2 = 0;
+					float b1, b2;
+
+					for( y = 0; y < winSize.height; y++ )
+					{
+						const uchar* Jptr = J.ptr() + (y + inextPt.y)*stepJ + inextPt.x*cn;
+						const short* Iptr = IWinBuf.ptr<short>(y);
+						const short* dIptr = derivIWinBuf.ptr<short>(y);
+
+						x = 0;
+
+						for( ; x < winSize.width*cn; x++, dIptr += 2 )
+						{
+							int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
+												  Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
+												  W_BITS1-5) - Iptr[x];
+							ib1 += (itemtype)(diff*dIptr[0]);
+							ib2 += (itemtype)(diff*dIptr[1]);
+						}
+					}
+
+					b1 = ib1*FLT_SCALE;
+					b2 = ib2*FLT_SCALE;
+
+					cv::Point2f delta( (float)((A12*b2 - A22*b1) * D),
+								  0);//(float)((A12*b1 - A11*b2) * D)); // MODIFICATION
+					//delta = -delta;
+
+					nextPt += delta;
+					nextPts[ptidx] = nextPt + halfWin;
+
+					if( delta.ddot(delta) <= criteria.epsilon )
+						break;
+
+					if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
+					   std::abs(delta.y + prevDelta.y) < 0.01 )
+					{
+						nextPts[ptidx] -= delta*0.5f;
+						break;
+					}
+					prevDelta = delta;
+				}
+
+				if( status[ptidx] && err && level == 0 && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) == 0 )
+				{
+					cv::Point2f nextPoint = nextPts[ptidx] - halfWin;
+					cv::Point inextPoint;
+
+					inextPoint.x = cvFloor(nextPoint.x);
+					inextPoint.y = cvFloor(nextPoint.y);
+
+					if( inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
+						inextPoint.y < -winSize.height || inextPoint.y >= J.rows )
+					{
+						if( status )
+							status[ptidx] = false;
+						continue;
+					}
+
+					float aa = nextPoint.x - inextPoint.x;
+					float bb = nextPoint.y - inextPoint.y;
+					iw00 = cvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
+					iw01 = cvRound(aa*(1.f - bb)*(1 << W_BITS));
+					iw10 = cvRound((1.f - aa)*bb*(1 << W_BITS));
+					iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
+					float errval = 0.f;
+
+					for( y = 0; y < winSize.height; y++ )
+					{
+						const uchar* Jptr = J.ptr() + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
+						const short* Iptr = IWinBuf.ptr<short>(y);
+
+						for( x = 0; x < winSize.width*cn; x++ )
+						{
+							int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
+												  Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
+												  W_BITS1-5) - Iptr[x];
+							errval += std::abs((float)diff);
+						}
+					}
+					err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
+				}
+			}
+        }
+
+    }
+}
 
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
@@ -88,40 +762,6 @@ cv::Mat disparityFromStereoImages(
 	return disparity;
 }
 
-cv::Mat disparityFromStereoImages(
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		int flowWinSize,
-		int flowMaxLevel,
-		int flowIterations,
-		double flowEps,
-		float maxCorrespondencesSlope)
-{
-	UASSERT(!leftImage.empty() && !rightImage.empty());
-	UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
-	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
-
-	// Find features in the new left image
-	std::vector<unsigned char> status;
-	std::vector<float> err;
-	std::vector<cv::Point2f> rightCorners;
-	UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-	cv::calcOpticalFlowPyrLK(
-			leftImage,
-			rightImage,
-			leftCorners,
-			rightCorners,
-			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");
-
-	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
-}
-
 cv::Mat depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,
 		int type)
@@ -205,25 +845,22 @@ cv::Mat depthFromStereoImages(
 }
 
 cv::Mat disparityFromStereoCorrespondences(
-		const cv::Mat & leftImage,
+		const cv::Size & disparitySize,
 		const std::vector<cv::Point2f> & leftCorners,
 		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask,
-		float maxSlope)
+		const std::vector<unsigned char> & mask)
 {
-	UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
+	UASSERT(leftCorners.size() == rightCorners.size());
 	UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
-	cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
+	cv::Mat disparity = cv::Mat::zeros(disparitySize, CV_32FC1);
 	for(unsigned int i=0; i<leftCorners.size(); ++i)
 	{
-		if(mask.size() == 0 || mask[i])
+		if(mask.empty() || mask[i])
 		{
-			float d = leftCorners[i].x - rightCorners[i].x;
-			float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
-			if(d > 0.0f && (maxSlope <= 0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxSlope))
-			{
-				disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
-			}
+			cv::Point2i dispPt(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f));
+			UASSERT(dispPt.x >= 0 && dispPt.x < disparitySize.width);
+			UASSERT(dispPt.y >= 0 && dispPt.y < disparitySize.height);
+			disparity.at<float>(dispPt.y, dispPt.x) = leftCorners[i].x - rightCorners[i].x;
 		}
 	}
 	return disparity;

corelib/src/util3d.cpp

@@ -358,8 +358,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 		const cv::Mat & imageDisparity,
-		float cx, float cy,
-		float fx, float baseline,
+		const StereoCameraModel & model,
 		int decimation)
 {
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
@@ -382,7 +381,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
 			{
 				float disp = float(imageDisparity.at<short>(h,w))/16.0f;
-				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
 			}
 		}
 	}
@@ -393,7 +392,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
 			{
 				float disp = imageDisparity.at<float>(h,w);
-				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
 			}
 		}
 	}
@@ -403,8 +402,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,
-		float cx, float cy,
-		float fx, float baseline,
+		const StereoCameraModel & model,
 		int decimation)
 {
 	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
@@ -453,7 +451,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 			}
 
 			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);
+			pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
 			pt.x = ptXYZ.x;
 			pt.y = ptXYZ.y;
 			pt.z = ptXYZ.z;
@@ -465,8 +463,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
-		float cx, float cy,
-		float fx, float baseline,
+		const StereoCameraModel & model,
 		int decimation)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
@@ -479,14 +476,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 	cv::Mat leftColor = imageLeft;
 	cv::Mat rightMono = imageRight;
 
+	StereoCameraModel modelDecimation = model;
+
 	if(leftColor.rows % decimation != 0 ||
 	   leftColor.cols % decimation != 0)
 	{
 		leftColor = util2d::decimate(leftColor, decimation);
 		rightMono = util2d::decimate(rightMono, decimation);
-		fx /= float(decimation);
-		cx /= float(decimation);
-		cy /= float(decimation);
+		modelDecimation.scale(1/float(decimation));
 		decimation = 1;
 	}
 
@@ -503,8 +500,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 	return cloudFromDisparityRGB(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono),
-			cx, cy,
-			fx, baseline,
+			modelDecimation,
 			decimation);
 }
 
@@ -595,10 +591,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		}
 		cloud = cloudFromDisparity(
 				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
-				sensorData.stereoCameraModel().left().cx(),
-				sensorData.stereoCameraModel().left().cy(),
-				sensorData.stereoCameraModel().left().fx(),
-				sensorData.stereoCameraModel().baseline(),
+				sensorData.stereoCameraModel(),
 				decimation);
 
 		if(cloud->size())
@@ -720,10 +713,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		UDEBUG("");
 		cloud = cloudFromStereoImages(sensorData.imageRaw(),
 				sensorData.rightRaw(),
-				sensorData.stereoCameraModel().left().cx(),
-				sensorData.stereoCameraModel().left().cy(),
-				sensorData.stereoCameraModel().left().fx(),
-				sensorData.stereoCameraModel().baseline(),
+				sensorData.stereoCameraModel(),
 				decimation);
 
 		if(cloud->size())
@@ -871,12 +861,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,
-		float cx, float cy, float fx, float baseline)
+		const StereoCameraModel & model)
 {
-	if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
+	if(disparity != 0.0f && model.baseline() > 0.0f && model.left().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);
+		//Z = baseline * f / (d + cx1-cx0);
+		float W = model.baseline()/(disparity + model.right().cx() - model.left().cx());
+		return pcl::PointXYZ((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
 	}
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
 	return pcl::PointXYZ(bad_point, bad_point, bad_point);
@@ -885,7 +876,7 @@ pcl::PointXYZ projectDisparityTo3D(
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,
 		const cv::Mat & disparity,
-		float cx, float cy, float fx, float baseline)
+		const StereoCameraModel & model)
 {
 	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
 	int u = int(pt.x+0.5f);
@@ -895,7 +886,7 @@ pcl::PointXYZ projectDisparityTo3D(
 	   uIsInBounds(v, 0, disparity.rows))
 	{
 		float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
-		return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
+		return projectDisparityTo3D(pt, d, model);
 	}
 	return pcl::PointXYZ(bad_point, bad_point, bad_point);
 }

corelib/src/util3d_features.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_features.h"
 
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_correspondences.h"
@@ -111,10 +112,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		pcl::PointXYZ pt = util3d::projectDisparityTo3D(
 				keypoints[i].pt,
 				disparity,
-				stereoCameraModel.left().cx(),
-				stereoCameraModel.left().cy(),
-				stereoCameraModel.left().fx(),
-				stereoCameraModel.baseline());
+				stereoCameraModel);
 
 		if(pcl::isFinite(pt) &&
 			!stereoCameraModel.left().localTransform().isNull() &&
@@ -127,105 +125,39 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 	return keypoints3d;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		Transform localTransform,
-		int flowWinSize,
-		int flowMaxLevel,
-		int flowIterations,
-		double flowEps,
-		double maxCorrespondencesSlope)
-{
-	std::vector<cv::Point2f> leftCorners;
-	cv::KeyPoint::convert(keypoints, leftCorners);
-	return generateKeypoints3DStereo(
-			leftCorners,
-			leftImage,
-			rightImage,
-			fx,
-			baseline,
-			cx,
-			cy,
-			localTransform,
-			flowWinSize,
-			flowMaxLevel,
-			flowIterations,
-			flowEps,
-			maxCorrespondencesSlope);
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		const std::vector<cv::Point2f> & leftCorners,
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		Transform localTransform,
-		int flowWinSize,
-		int flowMaxLevel,
-		int flowIterations,
-		double flowEps,
-		double maxCorrespondencesSlope)
+		const std::vector<cv::Point2f> & rightCorners,
+		const StereoCameraModel & model,
+		const std::vector<unsigned char> & mask)
 {
-	UASSERT(!leftImage.empty() && !rightImage.empty() &&
-			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
-			leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
-	UASSERT(fx > 0.0f && baseline > 0.0f);
-
-	// Find features in the new left image
-	std::vector<unsigned char> status;
-	std::vector<float> err;
-	std::vector<cv::Point2f> rightCorners;
-	UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-	cv::calcOpticalFlowPyrLK(
-			leftImage,
-			rightImage,
-			leftCorners,
-			rightCorners,
-			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");
+	UASSERT(leftCorners.size() == rightCorners.size());
+	UASSERT(mask.size() == 0 || leftCorners.size() == mask.size());
+	UASSERT(model.left().fx()> 0.0f && model.baseline() > 0.0f);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	keypoints3d->resize(leftCorners.size());
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
-	UASSERT(status.size() == leftCorners.size());
-	for(unsigned int i=0; i<status.size(); ++i)
+	for(unsigned int i=0; i<leftCorners.size(); ++i)
 	{
 		pcl::PointXYZ pt(bad_point, bad_point, bad_point);
-		if(status[i])
+		if(mask.empty() || mask[i])
 		{
 			float disparity = leftCorners[i].x - rightCorners[i].x;
-			float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
-			if(disparity > 0.0f &&
-			   (maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope))
+			if(disparity != 0.0f)
 			{
 				pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 						leftCorners[i],
 						disparity,
-						cx,
-						cy,
-						fx,
-						baseline);
+						model);
 
 				if(pcl::isFinite(tmpPt))
 				{
 					pt = tmpPt;
-					if(!localTransform.isNull() &&
-					   !localTransform.isIdentity())
+					if(!model.localTransform().isNull() &&
+					   !model.localTransform().isIdentity())
 					{
-						pt = util3d::transformPoint(pt, localTransform);
+						pt = util3d::transformPoint(pt, model.localTransform());
 					}
 				}
 			}

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/opencv.hpp>
 
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
 
 #include <rtabmap/utilite/UEventsHandler.h>
 

guilib/include/rtabmap/gui/ImageView.h

@@ -78,7 +78,7 @@ public:
 	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
-	void addLine(float x1, float y1, float x2, float y2, QColor color);
+	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
 	void setImage(const QImage & image);
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, QColor color);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -241,7 +241,7 @@ private slots:
 	void makeObsoleteCloudRenderingPanel();
 	void makeObsoleteLoggingPanel();
 	void makeObsoleteSourcePanel();
-	void clicked(const QModelIndex &index);
+	void clicked(const QModelIndex & current, const QModelIndex & previous);
 	void addParameter(int value);
 	void addParameter(bool value);
 	void addParameter(double value);

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "ui_createSimpleCalibrationDialog.h"
 
 #include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/StereoCameraModel.h"
 #include "rtabmap/utilite/ULogger.h"
 
 #include <QFileDialog>

guilib/src/DatabaseViewer.cpp

@@ -262,6 +262,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
 	connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
 	connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
+	connect(ui_->checkBox_stereo_opticalFlow, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
+	connect(ui_->checkBox_stereo_ssd, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
 	ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
 	ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
 	ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
@@ -334,6 +336,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_stereo_opticalFlow, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_stereo_ssd, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+
 	// dockwidget
 	QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
 	for(int i=0; i<dockWidgets.size(); ++i)
@@ -482,6 +487,8 @@ void DatabaseViewer::readSettings()
 	ui_->doubleSpinBox_stereo_gfttQuality->setValue(settings.value("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value()).toDouble());
 	ui_->doubleSpinBox_stereo_maxSlope->setValue(settings.value("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value()).toDouble());
 	ui_->checkBox_stereo_subpix->setChecked(settings.value("subpix", ui_->checkBox_stereo_subpix->isChecked()).toBool());
+	ui_->checkBox_stereo_opticalFlow->setChecked(settings.value("opticalFlow", ui_->checkBox_stereo_opticalFlow->isChecked()).toBool());
+	ui_->checkBox_stereo_ssd->setChecked(settings.value("ssd", ui_->checkBox_stereo_ssd->isChecked()).toBool());
 	settings.endGroup();
 
 	settings.endGroup(); // DatabaseViewer
@@ -588,6 +595,8 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value());
 	settings.setValue("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value());
 	settings.setValue("subpix", ui_->checkBox_stereo_subpix->isChecked());
+	settings.setValue("opticalFlow", ui_->checkBox_stereo_opticalFlow->isChecked());
+	settings.setValue("ssd", ui_->checkBox_stereo_ssd->isChecked());
 	settings.endGroup();
 
 	settings.endGroup(); // DatabaseViewer
@@ -2483,20 +2492,41 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 			UDEBUG("cv::cornerSubPix() end");
 		}
 
-		// Find features in the new left image
+		// Find features in the new right image
 		std::vector<unsigned char> status;
-		std::vector<float> err;
 		std::vector<cv::Point2f> rightCorners;
-		cv::calcOpticalFlowPyrLK(
-				leftMono,
-				data->depthOrRightRaw(),
-				leftCorners,
-				rightCorners,
-				status,
-				err,
-				cv::Size(ui_->spinBox_stereo_flowWinSize->value(), ui_->spinBox_stereo_flowWinSize->value()), ui_->spinBox_stereo_flowMaxLevel->value(),
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value()));
 
+		if(ui_->checkBox_stereo_opticalFlow->isChecked())
+		{
+			UDEBUG("");
+			std::vector<float> err;
+			util2d::calcOpticalFlowPyrLKStereo(
+					leftMono,
+					data->rightRaw(),
+					leftCorners,
+					rightCorners,
+					status,
+					err,
+					cv::Size(ui_->spinBox_stereo_flowWinSize->value(), 3), ui_->spinBox_stereo_flowMaxLevel->value(),
+					cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value()));
+			UDEBUG("");
+		}
+		else
+		{
+			UDEBUG("");
+			rightCorners = util2d::calcStereoCorrespondences(
+					leftMono,
+					data->rightRaw(),
+					leftCorners,
+					status,
+					cv::Size(ui_->spinBox_stereo_flowWinSize->value(), 3),
+					ui_->spinBox_stereo_flowMaxLevel->value(),
+					ui_->spinBox_stereo_flowIterations->value(),
+					0,
+					64,
+					ui_->checkBox_stereo_ssd->isChecked());
+			UDEBUG("");
+		}
 		float timeFlow = timer.ticks();
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2513,6 +2543,8 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 			pcl::PointXYZ pt(bad_point, bad_point, bad_point);
 			if(status[i])
 			{
+				//UDEBUG("Left(%f,%f), right(%f,%f) disparity=%f", leftCorners[i].x, leftCorners[i].y, rightCorners[i].x, rightCorners[i].y, leftCorners[i].x - rightCorners[i].x);
+
 				float disparity = leftCorners[i].x - rightCorners[i].x;
 				if(disparity > 0.0f)
 				{
@@ -2521,10 +2553,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 						pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 								leftCorners[i],
 								disparity,
-								data->stereoCameraModel().left().cx(),
-								data->stereoCameraModel().left().cy(),
-								data->stereoCameraModel().left().fx(),
-								data->stereoCameraModel().baseline());
+								data->stereoCameraModel());
 
 						if(pcl::isFinite(tmpPt))
 						{
@@ -2599,33 +2628,37 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		// Draw lines between corresponding features...
 		for(unsigned int i=0; i<kpts.size(); ++i)
 		{
-			QColor c = Qt::green;
-			if(status[i] == 0)
+			if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
 			{
-				c = Qt::red;
+				QColor c = Qt::green;
+				if(status[i] == 0)
+				{
+					c = Qt::red;
+				}
+				else if(status[i] == 100)
+				{
+					c = Qt::blue;
+				}
+				else if(status[i] == 101)
+				{
+					c = Qt::yellow;
+				}
+				else if(status[i] == 102)
+				{
+					c = Qt::magenta;
+				}
+				else if(status[i] == 110)
+				{
+					c = Qt::cyan;
+				}
+				ui_->graphicsView_stereo->addLine(
+						kpts[i].pt.x,
+						kpts[i].pt.y,
+						rightKpts[i].pt.x,
+						rightKpts[i].pt.y,
+						c,
+						QString("%1: (%2,%3) -> (%4,%5)").arg(i).arg(kpts[i].pt.x).arg(kpts[i].pt.y).arg(rightKpts[i].pt.x).arg(rightKpts[i].pt.y));
 			}
-			else if(status[i] == 100)
-			{
-				c = Qt::blue;
-			}
-			else if(status[i] == 101)
-			{
-				c = Qt::yellow;
-			}
-			else if(status[i] == 102)
-			{
-				c = Qt::magenta;
-			}
-			else if(status[i] == 110)
-			{
-				c = Qt::cyan;
-			}
-			ui_->graphicsView_stereo->addLine(
-					kpts[i].pt.x,
-					kpts[i].pt.y,
-					rightKpts[i].pt.x,
-					rightKpts[i].pt.y,
-					c);
 		}
 		ui_->graphicsView_stereo->update();
 	}

guilib/src/ImageView.cpp

@@ -36,12 +36,114 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QGraphicsEffect>
 #include <QInputDialog>
 #include <QVBoxLayout>
+#include <QGraphicsRectItem>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/core/util2d.h"
 
 namespace rtabmap {
 
+//LineItem
+class LineItem : public QGraphicsLineItem
+{
+public:
+	LineItem(float x1, float y1, float x2, float y2, const QString & text = QString(), QGraphicsItem * parent = 0) :
+		QGraphicsLineItem(x1, y1, x2, y2, parent),
+		_text(text),
+		_placeHolder(0)
+	{
+		this->setAcceptHoverEvents(true);
+		this->setFlag(QGraphicsItem::ItemIsFocusable, true);
+		_width = pen().width();
+	}
+	virtual ~LineItem()
+	{
+		if(_placeHolder)
+		{
+			delete _placeHolder;
+		}
+	}
+
+	void setColor(const QColor & color);
+
+protected:
+	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		QGraphicsScene * scene = this->scene();
+		if(scene && scene->focusItem() == 0)
+		{
+			this->showDescription();
+		}
+		else
+		{
+			this->setPen(QPen(pen().color(), _width+2));
+		}
+		QGraphicsLineItem::hoverEnterEvent(event);
+	}
+
+	virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		if(!this->hasFocus())
+		{
+			this->hideDescription();
+		}
+		QGraphicsLineItem::hoverEnterEvent(event);
+	}
+
+	virtual void focusInEvent ( QFocusEvent * event )
+	{
+		this->showDescription();
+		QGraphicsLineItem::focusInEvent(event);
+	}
+
+	virtual void focusOutEvent ( QFocusEvent * event )
+	{
+		this->hideDescription();
+		QGraphicsLineItem::focusOutEvent(event);
+	}
+
+private:
+	void showDescription()
+	{
+		if(!_text.isEmpty())
+		{
+			if(!_placeHolder)
+			{
+				_placeHolder = new QGraphicsRectItem (this);
+				_placeHolder->setVisible(false);
+				_placeHolder->setBrush(QBrush(QColor ( 0, 0, 0, 170 ))); // Black transparent background
+				QGraphicsTextItem * text = new QGraphicsTextItem(_placeHolder);
+				text->setDefaultTextColor(this->pen().color().rgb());
+				text->setPlainText(_text);
+				_placeHolder->setRect(text->boundingRect());
+			}
+
+			if(_placeHolder->parentItem())
+			{
+				_placeHolder->setParentItem(0); // Make it a to level item
+			}
+			_placeHolder->setZValue(this->zValue()+1);
+			_placeHolder->setPos(this->mapFromScene(0,0));
+			_placeHolder->setVisible(true);
+		}
+		QPen pen = this->pen();
+		this->setPen(QPen(pen.color(), _width+2));
+	}
+	void hideDescription()
+	{
+		if(_placeHolder)
+		{
+			_placeHolder->setVisible(false);
+		}
+		this->setPen(QPen(pen().color(), _width));
+	}
+
+private:
+	QString _text;
+	QGraphicsRectItem * _placeHolder;
+	int _width;
+};
+
 ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
 		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
@@ -595,10 +697,10 @@ void ImageView::addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor
 	}
 }
 
-void ImageView::addLine(float x1, float y1, float x2, float y2, QColor color)
+void ImageView::addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text)
 {
 	color.setAlpha(this->getAlpha());
-	QGraphicsLineItem * item  = new QGraphicsLineItem(x1, y1, x2, y2);
+	LineItem * item  = new LineItem(x1, y1, x2, y2, text);
 	item->setPen(QPen(color));
 	_lines.push_back(item);
 	item->setVisible(isLinesShown());

guilib/src/MainWindow.cpp

@@ -2959,7 +2959,7 @@ void MainWindow::startDetection()
 			return;
 		}
 
-		_camera = new CameraThread(camera);
+		_camera = new CameraThread(camera, parameters);
 		_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
 		_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
 		_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());

guilib/src/PreferencesDialog.cpp

@@ -673,10 +673,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
 
 	//Odometry Optical Flow
+	_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomFlowKeyFrameThr().c_str());
 	_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_guessMotion->setObjectName(Parameters::kOdomFlowGuessMotion().c_str());
 
 	//Odometry Mono
 	_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
@@ -693,12 +695,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_particleLambdaR->setObjectName(Parameters::kOdomParticleLambdaR().c_str());
 
 	//Stereo
-	_ui->stereo_flow_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
-	_ui->stereo_flow_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
-	_ui->stereo_flow_iterations->setObjectName(Parameters::kStereoIterations().c_str());
+	_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
+	_ui->stereo_winHeight->setObjectName(Parameters::kStereoWinHeight().c_str());
+	_ui->stereo_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
+	_ui->stereo_iterations->setObjectName(Parameters::kStereoIterations().c_str());
+	_ui->stereo_minDisparity->setObjectName(Parameters::kStereoMinDisparity().c_str());
+	_ui->stereo_maxDisparity->setObjectName(Parameters::kStereoMaxDisparity().c_str());
+	_ui->stereo_ssd->setObjectName(Parameters::kStereoSSD().c_str());
 	_ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str());
+	_ui->stereo_opticalFlow->setObjectName(Parameters::kStereoOpticalFlow().c_str());
 	_ui->stereo_maxSlope->setObjectName(Parameters::kStereoMaxSlope().c_str());
 
+	//StereoBM
+	_ui->stereobm_blockSize->setObjectName(Parameters::kStereoBMBlockSize().c_str());
+	_ui->stereobm_minDisparity->setObjectName(Parameters::kStereoBMMinDisparity().c_str());
+	_ui->stereobm_numDisparities->setObjectName(Parameters::kStereoBMNumDisparities().c_str());
+	_ui->stereobm_preFilterCap->setObjectName(Parameters::kStereoBMPreFilterCap().c_str());
+	_ui->stereobm_preFilterSize->setObjectName(Parameters::kStereoBMPreFilterSize().c_str());
+	_ui->stereobm_speckleRange->setObjectName(Parameters::kStereoBMSpeckleRange().c_str());
+	_ui->stereobm_speckleWinSize->setObjectName(Parameters::kStereoBMSpeckleWindowSize().c_str());
+	_ui->stereobm_tetureThreshold->setObjectName(Parameters::kStereoBMTextureThreshold().c_str());
+	_ui->stereobm_uniquessRatio->setObjectName(Parameters::kStereoBMUniquenessRatio().c_str());
+
+
 
 	setupSignals();
 	// custom signals
@@ -711,7 +730,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// This will add all parameters to the parameters Map
 	_ui->stackedWidget->setCurrentIndex(0);
 	this->setupTreeView();
-	connect(_ui->treeView, SIGNAL(clicked(QModelIndex)), this, SLOT(clicked(QModelIndex)));
 
 	_obsoletePanels = kPanelAll;
 
@@ -820,7 +838,10 @@ void PreferencesDialog::setupTreeView()
 	{
 		_ui->treeView->setCurrentIndex(_indexModel->index(currentIndex-2, 0));
 	}
-	_ui->treeView->expandToDepth(0);
+	_ui->treeView->expandToDepth(1);
+
+	// should be after setModel()
+	connect(_ui->treeView->selectionModel(), SIGNAL(currentChanged(const QModelIndex &, const QModelIndex &)), this, SLOT(clicked(const QModelIndex &, const QModelIndex &)));
 }
 
 // recursive...
@@ -933,9 +954,9 @@ void PreferencesDialog::setupSignals()
 	}
 }
 
-void PreferencesDialog::clicked(const QModelIndex &index)
+void PreferencesDialog::clicked(const QModelIndex & current, const QModelIndex & previous)
  {
-	QStandardItem * item = _indexModel->itemFromIndex(index);
+	QStandardItem * item = _indexModel->itemFromIndex(current);
 	if(item && item->isEnabled())
 	{
 		int index = item->data().toInt();
@@ -2045,7 +2066,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(false);
 
 		_ui->groupBox_source0->setEnabled(false);
-		_ui->groupBox_odometry2->setEnabled(false);
+		_ui->groupBox_odometry1->setEnabled(false);
 
 		this->setWindowTitle(tr("Preferences [Monitoring mode]"));
 	}
@@ -2060,7 +2081,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(true);
 
 		_ui->groupBox_source0->setEnabled(true);
-		_ui->groupBox_odometry2->setEnabled(true);
+		_ui->groupBox_odometry1->setEnabled(true);
 
 		this->setWindowTitle(tr("Preferences"));
 	}
@@ -4005,7 +4026,7 @@ void PreferencesDialog::testOdometry()
 
 	if(camera)
 	{
-		CameraThread cameraThread(camera); // take ownership of camera
+		CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera
 		cameraThread.setMirroringEnabled(isSourceMirroring());
 		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 		cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
@@ -4072,7 +4093,7 @@ void PreferencesDialog::testCamera()
 		Camera * camera = this->createCamera();
 		if(camera)
 		{
-			CameraThread cameraThread(camera);
+			CameraThread cameraThread(camera, this->getAllParameters());
 			cameraThread.setMirroringEnabled(isSourceMirroring());
 			cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
 			cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
@@ -4123,7 +4144,7 @@ void PreferencesDialog::calibrate()
 	_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 	_calibrationDialog->registerToEventsManager();
 
-	CameraThread cameraThread(camera);
+	CameraThread cameraThread(camera, this->getAllParameters());
 	UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
 
 	cameraThread.start();

guilib/src/ui/DatabaseViewer.ui

@@ -50,7 +50,7 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>196</width>
+             <width>200</width>
              <height>184</height>
             </rect>
            </property>
@@ -236,7 +236,7 @@
             <rect>
              <x>0</x>
              <y>0</y>
-             <width>195</width>
+             <width>199</width>
              <height>184</height>
             </rect>
            </property>
@@ -2061,15 +2061,86 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-57</y>
-          <width>283</width>
-          <height>356</height>
+          <y>-198</y>
+          <width>263</width>
+          <height>460</height>
          </rect>
         </property>
         <attribute name="label">
          <string>Stereo correspondences</string>
         </attribute>
         <layout class="QGridLayout" name="gridLayout_13">
+         <item row="7" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_flowEps">
+           <property name="decimals">
+            <number>4</number>
+           </property>
+           <property name="maximum">
+            <double>1.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.010000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="6" column="1">
+          <widget class="QLabel" name="label_58">
+           <property name="text">
+            <string>Iterations</string>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_54">
+           <property name="text">
+            <string>GFTT quality level</string>
+           </property>
+          </widget>
+         </item>
+         <item row="12" column="1">
+          <spacer name="verticalSpacer_6">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
+           </property>
+           <property name="sizeHint" stdset="0">
+            <size>
+             <width>20</width>
+             <height>40</height>
+            </size>
+           </property>
+          </spacer>
+         </item>
+         <item row="10" column="0">
+          <widget class="QCheckBox" name="checkBox_stereo_subpix">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_70">
+           <property name="text">
+            <string>GFTT max features</string>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="1">
+          <widget class="QLabel" name="label_59">
+           <property name="text">
+            <string>Epsilon</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="1">
+          <widget class="QLabel" name="label_57">
+           <property name="text">
+            <string>Max level</string>
+           </property>
+          </widget>
+         </item>
          <item row="2" column="0">
           <widget class="QSpinBox" name="spinBox_stereo_gfttBlockSize">
            <property name="suffix">
@@ -2102,13 +2173,6 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="1">
-          <widget class="QLabel" name="label_59">
-           <property name="text">
-            <string>Optical Flow eps</string>
-           </property>
-          </widget>
-         </item>
          <item row="2" column="1">
           <widget class="QLabel" name="label_55">
            <property name="text">
@@ -2122,7 +2186,7 @@
             <string> pixels</string>
            </property>
            <property name="minimum">
-            <number>3</number>
+            <number>6</number>
            </property>
            <property name="value">
             <number>16</number>
@@ -2132,14 +2196,14 @@
          <item row="4" column="1">
           <widget class="QLabel" name="label_56">
            <property name="text">
-            <string>Optical Flow win size</string>
+            <string>Win size</string>
            </property>
           </widget>
          </item>
          <item row="5" column="0">
           <widget class="QSpinBox" name="spinBox_stereo_flowMaxLevel">
            <property name="minimum">
-            <number>1</number>
+            <number>0</number>
            </property>
            <property name="value">
             <number>3</number>
@@ -2153,47 +2217,10 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_flowEps">
-           <property name="decimals">
-            <number>4</number>
-           </property>
-           <property name="maximum">
-            <double>1.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>0.010000000000000</double>
-           </property>
-           <property name="value">
-            <double>0.010000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLabel" name="label_57">
-           <property name="text">
-            <string>Optical Flow max level</string>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="1">
-          <widget class="QLabel" name="label_58">
-           <property name="text">
-            <string>Optical Flow iterations</string>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_54">
-           <property name="text">
-            <string>GFTT quality level</string>
-           </property>
-          </widget>
-         </item>
          <item row="6" column="0">
           <widget class="QSpinBox" name="spinBox_stereo_flowIterations">
            <property name="minimum">
-            <number>1</number>
+            <number>0</number>
            </property>
            <property name="maximum">
             <number>1000</number>
@@ -2232,7 +2259,7 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="1">
+         <item row="10" column="1">
           <widget class="QLabel" name="label_62">
            <property name="text">
             <string>Sub pixel</string>
@@ -2246,33 +2273,6 @@
            </property>
           </widget>
          </item>
-         <item row="10" column="1">
-          <spacer name="verticalSpacer_6">
-           <property name="orientation">
-            <enum>Qt::Vertical</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>20</width>
-             <height>40</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item row="9" column="0">
-          <widget class="QCheckBox" name="checkBox_stereo_subpix">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="1">
-          <widget class="QLabel" name="label_70">
-           <property name="text">
-            <string>GFTT max features</string>
-           </property>
-          </widget>
-         </item>
          <item row="3" column="0">
           <widget class="QSpinBox" name="spinBox_stereo_gfttMaxFeatures">
            <property name="suffix">
@@ -2292,6 +2292,40 @@
            </property>
           </widget>
          </item>
+         <item row="9" column="1">
+          <widget class="QLabel" name="label_71">
+           <property name="text">
+            <string>Optical Flow</string>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="0">
+          <widget class="QCheckBox" name="checkBox_stereo_opticalFlow">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="11" column="1">
+          <widget class="QLabel" name="label_72">
+           <property name="text">
+            <string>SSD (otherwise SAD is used)</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="11" column="0">
+          <widget class="QCheckBox" name="checkBox_stereo_ssd">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
         </layout>
        </widget>
       </widget>

tools/CMakeLists.txt

@@ -4,6 +4,7 @@ ADD_SUBDIRECTORY( ImagesJoiner )
 ADD_SUBDIRECTORY( ExtractObject )
 ADD_SUBDIRECTORY( Camera )
 ADD_SUBDIRECTORY( CameraRGBD )
+ADD_SUBDIRECTORY( StereoEval )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/CameraRGBD/main.cpp

@@ -377,10 +377,7 @@ int main(int argc, char * argv[])
 				}
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
 						rgb, right,
-						data.stereoCameraModel().left().cx(),
-						data.stereoCameraModel().left().cy(),
-						data.stereoCameraModel().left().fx(),
-						data.stereoCameraModel().baseline());
+						data.stereoCameraModel());
 				cloud = rtabmap::util3d::transformPointCloud(cloud, t);
 				if(viewer)
 					viewer->showCloud(cloud, "cloud");

tools/StereoEval/CMakeLists.txt

@@ -0,0 +1,22 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(stereoEval main.cpp)
+TARGET_LINK_LIBRARIES(stereoEval rtabmap_core rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( stereoEval 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-stereoEval)
+ 

tools/StereoEval/io.h

@@ -0,0 +1,163 @@
+/*
+ * pfmReader.h
+ *
+ *  Created on: Dec 4, 2015
+ *      Author: mathieu
+ */
+
+#ifndef PFMREADER_H_
+#define PFMREADER_H_
+
+#include <iostream>
+#include <opencv2/opencv.hpp>
+
+// taken from http://vision.middlebury.edu/stereo/ evaluation tool
+
+void skipComment(FILE *fp)
+{
+    // skip comment lines in the headers of pnm files
+
+    char c;
+    while ((c=getc(fp)) == '#')
+        while (getc(fp) != '\n') ;
+    ungetc(c, fp);
+}
+
+void skipSpace(FILE *fp)
+{
+    // skip white space in the headers or pnm files
+
+    char c;
+    do {
+        c = getc(fp);
+    } while (c == '\n' || c == ' ' || c == '\t' || c == '\r');
+    ungetc(c, fp);
+}
+
+bool readHeader(FILE *fp, const char *imtype, char c1, char c2,
+                 int *width, int *height, int *nbands, int thirdArg)
+{
+    // read the header of a pnmfile and initialize width and height
+
+    char c;
+
+	if (getc(fp) != c1 || getc(fp) != c2)
+	{
+		printf("ReadFilePGM: wrong magic code for %s file\n", imtype);
+		return false;
+	}
+	skipSpace(fp);
+	skipComment(fp);
+	skipSpace(fp);
+	fscanf(fp, "%d", width);
+	skipSpace(fp);
+	fscanf(fp, "%d", height);
+	if (thirdArg) {
+		skipSpace(fp);
+		fscanf(fp, "%d", nbands);
+	}
+    // skip SINGLE newline character after reading image height (or third arg)
+	c = getc(fp);
+    if (c == '\r')      // <cr> in some files before newline
+        c = getc(fp);
+    if (c != '\n') {
+        if (c == ' ' || c == '\t' || c == '\r')
+        {
+            printf("newline expected in file after image height\n");
+            return false;
+        }
+        else
+        {
+        	printf("whitespace expected in file after image height\n");
+        	return false;
+        }
+  }
+
+    return true;
+}
+
+int littleendian()
+{
+    int intval = 1;
+    uchar *uval = (uchar *)&intval;
+    return uval[0] == 1;
+}
+
+cv::Mat readPFM(const char* filename)
+{
+	cv::Mat disp;
+
+    // Open the file and read the header
+    FILE *fp = fopen(filename, "rb");
+    if (fp == 0)
+    {
+        printf("ReadFilePFM: could not open %s\n", filename);
+        return cv::Mat();
+    }
+
+    int width, height, nBands;
+    readHeader(fp, "PFM", 'P', 'f', &width, &height, &nBands, 0);
+
+    skipSpace(fp);
+
+    float scalef;
+    fscanf(fp, "%f", &scalef);  // scale factor (if negative, little endian)
+
+    // skip SINGLE newline character after reading third arg
+    char c = getc(fp);
+    if (c == '\r')      // <cr> in some files before newline
+        c = getc(fp);
+    if (c != '\n') {
+        if (c == ' ' || c == '\t' || c == '\r')
+        {
+            printf("newline expected in file after scale factor\n");
+            return cv::Mat();
+        }
+        else
+        {
+            printf("whitespace expected in file after scale factor\n");
+            return cv::Mat();
+        }
+    }
+
+    // Set the image shape
+    disp = cv::Mat(height, width, CV_32FC1);
+
+    int littleEndianFile = (scalef < 0);
+    int littleEndianMachine = littleendian();
+    int needSwap = (littleEndianFile != littleEndianMachine);
+    //printf("endian file = %d, endian machine = %d, need swap = %d\n",
+    //       littleEndianFile, littleEndianMachine, needSwap);
+
+    for (int y = height-1; y >= 0; y--) { // PFM stores rows top-to-bottom!!!!
+	int n = width;
+	float* ptr = (float *) disp.row(y).data;
+	if ((int)fread(ptr, sizeof(float), n, fp) != n)
+	{
+	    printf("ReadFilePFM(%s): file is too short\n", filename);
+	    return cv::Mat();
+	}
+
+	if (needSwap) { // if endianness doesn't agree, swap bytes
+	    uchar* ptr = (uchar *) disp.row(y).data;
+	    int x = 0;
+	    uchar tmp = 0;
+	    while (x < n) {
+		tmp = ptr[0]; ptr[0] = ptr[3]; ptr[3] = tmp;
+		tmp = ptr[1]; ptr[1] = ptr[2]; ptr[2] = tmp;
+		ptr += 4;
+		x++;
+	    }
+	}
+    }
+    if (fclose(fp))
+    {
+        printf("ReadFilePGM(%s): error closing file\n", filename);
+        return cv::Mat();
+    }
+
+    return disp;
+}
+
+
+#endif /* PFMREADER_H_ */

tools/StereoEval/main.cpp

@@ -0,0 +1,428 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "io.h"
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Stereo.h>
+#include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMath.h>
+#include <fstream>
+#include <string>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"evalStereo.exe left.png right.png calib.txt disp.pfm mask.png [Parameters]\n"
+			"Example (with http://vision.middlebury.edu/stereo datasets):\n"
+			"  $ ./rtabmap-stereoEval im0.png im1.png calib.txt disp0GT.pfm mask0nocc.png -Kp/DetectorStrategy 6 -Stereo/WinSize 5 -Stereo/MaxLevel 2 -Kp/WordsPerImage 1000 -Stereo/OpticalFlow false -Stereo/Iterations 5\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setLevel(ULogger::kDebug);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	if(argc < 6)
+	{
+		showUsage();
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	for(int i=6; i<argc; ++i)
+	{
+		// Check for RTAB-Map's parameters
+		std::string key = argv[i];
+		key = uSplit(key, '-').back();
+		if(parameters.find(key) != parameters.end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+				std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
+				if(inserted.second == false)
+				{
+					inserted.first->second = value;
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		//backward compatibility
+		// look for old parameter name
+		std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
+		if(oldIter!=Parameters::getRemovedParameters().end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+
+				if(oldIter->second.first)
+				{
+					key = oldIter->second.second;
+					UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+							oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
+				}
+				else if(oldIter->second.second.empty())
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
+				}
+				else
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
+				}
+				if(oldIter->second.first)
+				{
+					std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
+					if(inserted.second == false)
+					{
+						inserted.first->second = value;
+					}
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	UINFO("Loading files...");
+
+	cv::Mat left = cv::imread(argv[1]);
+	cv::Mat right = cv::imread(argv[2]);
+	cv::Mat disp = readPFM(argv[4]);
+	cv::Mat mask = cv::imread(argv[5]);
+
+	if(!left.empty() && !right.empty() && !disp.empty() && !mask.empty())
+	{
+		UASSERT(left.rows == disp.rows);
+		UASSERT(left.cols == disp.cols);
+		UASSERT(disp.rows == mask.rows);
+		UASSERT(disp.cols == mask.cols);
+
+		// read calib.txt
+		// Example format:
+		// --- calib.txt:
+		// cam0=[1038.018 0 322.037; 0 1038.018 243.393; 0 0 1]
+		// cam1=[1038.018 0 375.308; 0 1038.018 243.393; 0 0 1]
+		// doffs=53.271
+		// baseline=176.252
+		// width=718
+		// height=496
+		// ndisp=73
+		// isint=0
+		// vmin=8
+		// vmax=65
+		// dyavg=0.184
+		// dymax=0.423
+		// ---
+		std::string calibFile = argv[3];
+		std::ifstream stream(calibFile);
+		std::string line;
+
+		// two first lines are camera intrinsics
+		UINFO("Loading calibration... (%s)", calibFile.c_str());
+		std::vector<cv::Mat> K(2);
+		for(int i=0; i<2; ++i)
+		{
+			getline(stream, line);
+			line.erase(0, 6);
+			line = uReplaceChar(line, ']', "");
+			line = uReplaceChar(line, ';', "");
+			UINFO("K[%d] = %s", i, line.c_str());
+			std::vector<std::string> valuesStr = uListToVector(uSplit(line, ' '));
+			UASSERT(valuesStr.size() == 9);
+			K[i] = cv::Mat(3,3,CV_64FC1);
+			for(unsigned int j=0; j<valuesStr.size(); ++j)
+			{
+				K[i].at<double>(j) = uStr2Double(valuesStr[j]);
+			}
+		}
+
+		// skip doffs line
+		getline(stream, line);
+
+		// baseline
+		getline(stream, line);
+		line.erase(0, 9);
+		double baseline = uStr2Double(line);
+		UINFO("Baseline = %f", baseline);
+
+		StereoCameraModel model(
+				calibFile,
+				CameraModel(K[0].at<double>(0,0), K[0].at<double>(1,1), K[0].at<double>(0,2), K[0].at<double>(1,2)),
+				CameraModel(K[1].at<double>(0,0), K[1].at<double>(1,1), K[1].at<double>(0,2), K[1].at<double>(1,2), Transform::getIdentity(), -baseline/K[1].at<double>(0,0)));
+
+		UASSERT(model.isValid());
+
+		UINFO("Processing...");
+
+		// Processing...
+		cv::Mat leftMono;
+		if(left.channels() == 3)
+		{
+			cv::cvtColor(left, leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = left;
+		}
+		cv::Mat rightMono;
+		if(right.channels() == 3)
+		{
+			cv::cvtColor(right, rightMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			rightMono = right;
+		}
+
+		UTimer timer;
+		double timeKpts;
+		double timeSubPixel;
+		double timeStereo;
+
+		// generate kpts
+		std::vector<cv::KeyPoint> kpts;
+		cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
+		int type;
+		Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
+		Feature2D * kptDetector = Feature2D::create(Feature2D::Type(type), parameters);
+		kpts = kptDetector->generateKeypoints(leftMono, roi);
+		delete kptDetector;
+
+		timeKpts = timer.ticks();
+
+		std::vector<cv::Point2f> leftCorners(kpts.size());
+		cv::KeyPoint::convert(kpts, leftCorners);
+		int subPixWinSize = 0;
+		int subPixIterations = 0;
+		double subPixEps = 0;
+		Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), subPixWinSize);
+		Parameters::parse(parameters, Parameters::kKpSubPixIterations(), subPixIterations);
+		Parameters::parse(parameters, Parameters::kKpSubPixEps(), subPixEps);
+		if(subPixWinSize > 0 && subPixIterations > 0)
+		{
+			UDEBUG("cv::cornerSubPix() begin");
+			cv::cornerSubPix(leftMono, leftCorners,
+				cv::Size( subPixWinSize, subPixWinSize ),
+				cv::Size( -1, -1 ),
+				cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
+			UDEBUG("cv::cornerSubPix() end");
+		}
+
+		timeSubPixel = timer.ticks();
+
+		// Find features in the new right image
+		std::vector<unsigned char> status;
+		std::vector<cv::Point2f> rightCorners;
+
+		bool opticalFlow = false;
+		Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
+		Stereo * stereo = 0;
+		if(opticalFlow)
+		{
+			stereo = new StereoOpticalFlow(parameters);
+		}
+		else
+		{
+			stereo = new Stereo(parameters);
+		}
+
+		rightCorners = stereo->computeCorrespondences(
+				leftMono,
+				rightMono,
+				leftCorners,
+				status);
+		delete stereo;
+
+		timeStereo = timer.ticks();
+
+		UINFO("Time: kpts:%f s, subpix=%f s, stereo=%f s", timeKpts, timeSubPixel, timeStereo);
+
+		UDEBUG("Mask = %d", mask.type());
+
+		int inliers = 0;
+		int subInliers = 0;
+		int badInliers = 0;
+		int outliers = 0;
+		float sumInliers = 0.0f;
+		float sumSubInliers = 0.0f;
+		int goodRejected = 0;
+		int badRejected = 0;
+		for(unsigned int i=0; i<leftCorners.size(); ++i)
+		{
+			float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
+			if(status[i]!=0)
+			{
+				float d = leftCorners[i].x - rightCorners[i].x;
+				//float err = fabs(d-gt);
+				//UDEBUG("Pt(%f,%f): d=%f, gt=%f, error=%f", leftCorners[i].x, leftCorners[i].y, d, gt, err);
+
+				if(uIsFinite(gt))
+				{
+					if(fabs(d-gt) < 1.0f)
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 0, 255, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+
+						++inliers;
+						sumInliers += fabs(d-gt);
+
+						if(fabs(d-gt) < 0.5f)
+						{
+							++subInliers;
+							sumSubInliers += fabs(d-gt);
+						}
+					}
+					else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 0, 255, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+						++badInliers;
+						//UDEBUG("should be rejected or refined: %d pt=(%f,%f) (d=%f gt=%f)", i, leftCorners[i].x, leftCorners[i].y, d, gt);
+					}
+					else
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 255, 0, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+						++outliers;
+						//UDEBUG("should be rejected: %d", i);
+					}
+				}
+			}
+			else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
+					rightCorners[i].x > 0.0f)
+			{
+				float d = leftCorners[i].x - rightCorners[i].x;
+				cv::line(left,
+					leftCorners[i],
+					cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+					cv::Scalar( 0, 255, 255 ));
+
+				cv::line(left,
+						cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+						cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+					cv::Scalar( 0, 0, 255 ));
+				++goodRejected;
+				//UDEBUG("should not be rejected: %d", i);
+			}
+			else
+			{
+				++badRejected;
+				//UDEBUG("correctly rejected: %d", i);
+			}
+		}
+
+		UINFO("inliers=%d (%d%%) subInliers=%d (%d%%) bad inliers=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
+				inliers,
+				(inliers*100)/leftCorners.size(),
+				subInliers,
+				(subInliers*100)/leftCorners.size(),
+				badInliers,
+				(badInliers*100)/leftCorners.size(),
+				outliers,
+				(outliers*100)/leftCorners.size(),
+				goodRejected,
+				(goodRejected*100)/leftCorners.size(),
+				badRejected,
+				(badRejected*100)/leftCorners.size());
+		UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
+
+
+		cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Right", right );
+
+		cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Mask", mask );
+
+		cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Left", left );
+
+		cv::waitKey(0);
+
+	}
+
+	return 0;
+}

utilite/include/rtabmap/utilite/UConversion.h

@@ -117,6 +117,12 @@ std::string UTILITE_EXP uNumber2Str(float number);
  */
 std::string UTILITE_EXP uNumber2Str(double number);
 
+/**
+ * Convert a string to an integer.
+ * @param the string
+ * @return the number
+ */
+int UTILITE_EXP uStr2Int(const std::string & str);
 
 /**
  * Convert a string to a float independent of the locale (comma/dot).

utilite/src/UConversion.cpp

@@ -113,6 +113,11 @@ std::string uNumber2Str(double number)
 	return s.str();
 }
 
+int uStr2Int(const std::string & str)
+{
+	return atoi(str.c_str());
+}
+
 float uStr2Float(const std::string & str)
 {
 	float value = 0.0f;