0.11.7: Added ZED sdk support

corelib/include/rtabmap/core/CameraStereo.h

@@ -39,6 +39,14 @@ namespace FlyCapture2
 class Camera;
 }
 
+namespace sl
+{
+namespace zed
+{
+class Camera;
+}
+}
+
 namespace rtabmap
 {
 
@@ -94,6 +102,32 @@ private:
 	void * triclopsCtx_; // TriclopsContext
 };
 
+/////////////////////////
+// CameraStereoZED
+/////////////////////////
+class RTABMAP_EXP CameraStereoZed :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoZed(bool rgbdMode, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoZed();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	sl::zed::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	bool rgbdMode_;
+};
+
 /////////////////////////
 // CameraStereoImages
 /////////////////////////
@@ -147,6 +181,10 @@ public:
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -162,6 +200,8 @@ private:
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -221,9 +221,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #ifdef RTABMAP_NONFREE
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
 #else
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 2,            "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
 #endif
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, true, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
@@ -396,7 +396,17 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
-	RTABMAP_PARAM(Vis, FeatureType,              int, 6,        "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
+#ifndef RTABMAP_NONFREE
+#ifdef RTABMAP_OPENCV3
+	// OpenCV 3 without xFeatures2D module doesn't have BRIEF
+	RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#endif
+#else
+	RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB.");
+#endif
+	
 	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
 	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
 	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");

corelib/include/rtabmap/core/util2d.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 
 namespace rtabmap
 {
@@ -68,7 +69,7 @@ void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputA
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
+	    const ParametersMap & parameters = ParametersMap());
 
 cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,

corelib/include/rtabmap/core/util3d.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/TextureMesh.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 #include <map>
 #include <list>
@@ -116,14 +117,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -143,7 +146,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,

corelib/src/CMakeLists.txt

@@ -209,6 +209,27 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
+IF(ZED_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ZED_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ZED_LIBRARIES}
+	)
+	IF(CUDA_FOUND)
+		SET(INCLUDE_DIRS
+			${INCLUDE_DIRS}
+			${CUDA_INCLUDE_DIRS}
+		)
+		SET(LIBRARIES
+			${LIBRARIES}
+			${CUDA_LIBRARIES}
+		)
+	ENDIF(CUDA_FOUND)
+ENDIF(ZED_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/CameraRGB.cpp

@@ -722,7 +722,7 @@ CameraVideo::~CameraVideo()
 
 bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	_guid.clear();
+	_guid = cameraName;
 	if(_capture.isOpened())
 	{
 		_capture.release();
@@ -750,19 +750,22 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 	}
 	else
 	{
-		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
-		if(guid != 0 && guid != 0xffffffff)
+		if (_guid.empty())
 		{
-			_guid = uFormat("%08x", guid);
+			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				_guid = uFormat("%08x", guid);
+			}
 		}
 
 		// look for calibration files
-		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		if(!calibrationFolder.empty() && !_guid.empty())
 		{
-			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
+			if(!_model.load(calibrationFolder, _guid))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
+						_guid.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{

corelib/src/CameraStereo.cpp

@@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fc2triclops.h>
 #endif
 
+#ifdef RTABMAP_ZED
+#include <zed/Camera.hpp>
+#endif
+
 namespace rtabmap
 {
 
@@ -731,6 +735,152 @@ SensorData CameraStereoFlyCapture2::captureImage()
 	return data;
 }
 
+//
+// CameraStereoZED
+//
+bool CameraStereoZed::available()
+{
+#ifdef RTABMAP_ZED
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(bool rgbdMode, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		zed_(0),
+		rgbdMode_(rgbdMode)
+{
+}
+
+CameraStereoZed::~CameraStereoZed()
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+	}
+#endif
+}
+
+bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	
+	if(zed_->isZEDconnected())
+	{
+		zed_ = new sl::zed::Camera(sl::zed::HD720); // Use in Live Mode
+		//zed_ = new sl::zed::Camera(argv[1]); // Use in SVO playback mode
+
+		int width = zed_->getImageSize().width;
+		int height = zed_->getImageSize().height;
+
+		//init WITH self-calibration (- last parameter to false -)
+		sl::zed::ERRCODE err = zed_->init(sl::zed::MODE::PERFORMANCE, 0, true, false, false);
+
+		// Quit if an error occurred
+		if (err != sl::zed::SUCCESS)
+		{
+			UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err));
+			delete zed_;
+			zed_ = 0;
+			return false;
+		}
+	}
+	else
+	{
+		UERROR("ZED camera initialization failed: ZED is not connected!");
+		return false;
+	}
+	
+	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
+	sl::zed::resolution res = zed_->getImageSize();
+				
+	stereoModel_ = StereoCameraModel(
+		stereoParams->LeftCam.fx, 
+		stereoParams->LeftCam.fy, 
+		stereoParams->LeftCam.cx, 
+		stereoParams->LeftCam.cy, 
+		stereoParams->baseline/1000.0f,
+		this->getLocalTransform(),
+		cv::Size(res.width, res.height));
+
+	return true;
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZed::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoZed::getSerial() const
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getZEDSerial());
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoZed::captureImage()
+{
+	SensorData data;
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		sl::zed::SENSING_MODE dm_type = sl::zed::RAW;
+		bool res = zed_->grab(dm_type);
+
+		if(!res)
+		{
+			// get left image
+			cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_LEFT)));
+			cv::Mat left;
+			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
+
+			if(rgbdMode_)
+			{
+				// get depth image
+				cv::Mat depth;
+				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				depth /= 1000.0;
+
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+			}
+			else
+			{
+				// get right image
+				cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_RIGHT)));
+				cv::Mat right;
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+			
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+			}
+		}
+		else
+		{
+			UERROR("CameraStereoZed: Failed to grab images!");
+		}
+	}
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return data;
+}
+
+
 //
 // CameraStereoImages
 //
@@ -921,7 +1071,21 @@ CameraStereoVideo::CameraStereoVideo(
 		const Transform & localTransform) :
 		Camera(imageRate, localTransform),
 		path_(path),
-		rectifyImages_(rectifyImages)
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int device,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	path_(""),
+	rectifyImages_(false),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(device)
 {
 }
 
@@ -932,29 +1096,51 @@ CameraStereoVideo::~CameraStereoVideo()
 
 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	cameraName_ = cameraName;
 	if(capture_.isOpened())
 	{
 		capture_.release();
 	}
-	ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str());
-	capture_.open(path_.c_str());
+
+	if (src_ == CameraVideo::kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		capture_.open(usbDevice_);
+	}
+	else if (src_ == CameraVideo::kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		capture_.open(path_.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
+	}
 
 	if(!capture_.isOpened())
 	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
 		return false;
 	}
 	else
 	{
-		// look for calibration files
-		cameraName_ = cameraName;
-		if(!calibrationFolder.empty() && !cameraName.empty())
+		if (cameraName_.empty())
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName))
+			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				cameraName_ = uFormat("%08x", guid);
+			}
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && !cameraName_.empty())
+		{
+			if(!stereoModel_.load(calibrationFolder, cameraName_))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.c_str(), calibrationFolder.c_str());
+					cameraName_.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
@@ -1007,7 +1193,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightCvt = true;
 			}
 
-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);

corelib/src/Memory.cpp

@@ -3425,7 +3425,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
 		float decimationRatio = preDecimation / _imagePostDecimation;
-		double log2value = log(preDecimation)/log(2);
+		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];

corelib/src/Odometry.cpp

@@ -253,7 +253,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 
 		// transform back the keypoints in the original image
 		std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
-		double log2value = log(_imageDecimation)/log(2);
+		double log2value = log(double(_imageDecimation))/log(2.0);
 		for(unsigned int i=0; i<kpts.size(); ++i)
 		{
 			kpts[i].pt.x *= _imageDecimation;

corelib/src/util2d.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/StereoDense.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -724,12 +725,11 @@ void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextIm
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type)
+		const ParametersMap & parameters)
 {
 	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);
-	UASSERT(type == CV_32FC1 || type == CV_16SC1);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -741,32 +741,9 @@ cv::Mat disparityFromStereoImages(
 		leftMono = leftImage;
 	}
 	cv::Mat disparity;
-#if CV_MAJOR_VERSION < 3
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
-	stereo.state->SADWindowSize = 15;
-	stereo.state->minDisparity = 0;
-	stereo.state->numberOfDisparities = 64;
-	stereo.state->preFilterSize = 9;
-	stereo.state->preFilterCap = 31;
-	stereo.state->uniquenessRatio = 15;
-	stereo.state->textureThreshold = 10;
-	stereo.state->speckleWindowSize = 100;
-	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, type);
-#else
-	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
-	stereo->setBlockSize(15);
-	stereo->setMinDisparity(0);
-	stereo->setNumDisparities(64);
-	stereo->setPreFilterSize(9);
-	stereo->setPreFilterCap(31);
-	stereo->setUniquenessRatio(15);
-	stereo->setTextureThreshold(10);
-	stereo->setSpeckleWindowSize(100);
-	stereo->setSpeckleRange(4);
-	stereo->compute(leftMono, rightImage, disparity);
-#endif
-	return disparity;
+
+	StereoBM stereo(parameters);
+	return stereo.computeDisparity(leftMono, rightImage);
 }
 
 cv::Mat depthFromDisparity(const cv::Mat & disparity,

corelib/src/util3d.cpp

@@ -588,7 +588,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
@@ -623,7 +624,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 
 	return cloudFromDisparityRGB(
 			leftColor,
-			util2d::disparityFromStereoImages(leftMono, rightMono),
+			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
 			modelDecimation,
 			decimation,
 			maxDepth,
@@ -636,7 +637,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -696,7 +698,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
@@ -719,7 +721,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -804,7 +807,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				decimation,
 				maxDepth,
 				minDepth,
-				validIndices);
+				validIndices,
+				parameters);
 
 		if(cloud->size())
 		{