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,