Adding support for compressed input data and right color image. (#1463)

corelib/src/Features2d.cpp

@@ -908,25 +908,36 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 				if(d_imageLeft.empty()) {
 					d_imageLeft = cv::cuda::GpuMat(imageLeft);
 				}
-				// convert to grayscale
+				// convert to grayscale if not already
 				if(d_imageLeft.channels() > 1) {
 					cv::cuda::GpuMat tmp;
 					cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY);
 					d_imageLeft = tmp;
 				}
+
 				d_imageRight = data.depthOrRightRawGpu();
 				if(d_imageRight.empty()) {
 					d_imageRight = cv::cuda::GpuMat(imageRight);
 				}
+				// convert to grayscale if not already
+				if(d_imageRight.channels() > 1) {
+					cv::cuda::GpuMat tmp;
+					cv::cuda::cvtColor(d_imageRight, tmp, cv::COLOR_BGR2GRAY);
+					d_imageRight = tmp;
+				}
 			}
 			else
 #endif
 			{
-				// convert to grayscale (right image should be already grayscale)
+				// convert to grayscale
 				if(imageLeft.channels() > 1)
 				{
 					cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
 				}
+				if(imageRight.channels() > 1)
+				{
+					cv::cvtColor(data.rightRaw(), imageRight, cv::COLOR_BGR2GRAY);
+				}
 			}
 
 			std::vector<cv::Point2f> leftCorners;

corelib/src/Memory.cpp

@@ -4519,17 +4519,55 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 {
 	UDEBUG("");
 	SensorData data = inputData;
+
+	bool isIntermediateNode = data.id() < 0;
+
+	// uncompress data if needed
+	
+	if(!isIntermediateNode)
+	{
+		// We need raw images if we need to extract features and/or do tag detection
+		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
+			(!_useOdometryFeatures ||
+			 data.keypoints().empty() ||
+			 (int)data.keypoints().size() != data.descriptors().rows ||
+			 data.descriptors().empty() ||
+			 _detectMarkers ||
+			 _rotateImagesUpsideUp ||
+			 _imagePostDecimation > 1 ||
+			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
+
+		// Note: we could avoid uncompressing scan if we don't do any filtering
+		// and if we don't use it for local occupancy grid
+		bool needRawScan = true;
+
+		if( (needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty()) ||
+			(needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
+			(needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
+		{
+			cv::Mat left, right;
+			LaserScan laserScan;
+			UDEBUG("Uncompressing data...");
+			data.uncompressData(
+				needRawImages && data.imageRaw().empty() && !data.imageCompressed().empty() ? &left : 0,
+				needRawImages && data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty() ? &right : 0,
+				needRawScan && data.laserScanRaw().empty() && !data.laserScanCompressed().empty() ? &laserScan : 0);
+			UDEBUG("Uncompressing data...done!");
+		}
+	}
+
 	UASSERT(data.imageRaw().empty() ||
 			data.imageRaw().type() == CV_8UC1 ||
 			data.imageRaw().type() == CV_8UC3);
 	UASSERT_MSG(data.depthOrRightRaw().empty() ||
 			(  ( data.depthOrRightRaw().type() == CV_16UC1 ||
 				 data.depthOrRightRaw().type() == CV_32FC1 ||
-				 data.depthOrRightRaw().type() == CV_8UC1)
+				 data.depthOrRightRaw().type() == CV_8UC1 ||
+				 data.depthOrRightRaw().type() == CV_8UC3)
 			   &&
-				( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
+				( (data.imageRaw().empty() && !(data.depthOrRightRaw().type() == CV_8UC1 || data.depthOrRightRaw().type() == CV_8UC3)) ||
 				  (data.depthOrRightRaw().rows <= data.imageRaw().rows && data.depthOrRightRaw().cols <= data.imageRaw().cols))),
-				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). "
+				uFormat("image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d-%d(stereo)]). "
 						"For stereo, left and right images should be same size. "
 						"For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
 						data.imageRaw().cols,
@@ -4540,7 +4578,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						data.depthOrRightRaw().cols,
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
-						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
+						CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().empty() &&
@@ -4559,7 +4597,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	float t;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	bool isIntermediateNode = data.id() < 0;
 	int id = data.id();
 	if(_generateIds)
 	{
@@ -5754,8 +5791,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}
 
-	// Filter the laser scan?
 	LaserScan laserScan = data.laserScanRaw();
+	// Filter the laser scan?
 	if(!isIntermediateNode && laserScan.size())
 	{
 		if(laserScan.rangeMax() == 0.0f)
@@ -5902,12 +5939,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5915,20 +5952,20 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
-						compressedDepth,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
 						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.maxPoints(),
 								laserScan.rangeMax(),
 								laserScan.format(),
 								laserScan.localTransform()):
-							LaserScan(compressedScan,
+							LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 								laserScan.format(),
 								laserScan.rangeMin(),
 								laserScan.rangeMax(),
@@ -5936,8 +5973,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.angleMax(),
 								laserScan.angleIncrement(),
 								laserScan.localTransform()),
-						compressedImage,
-						compressedDepth,
+						compressedImage.empty()?data.imageCompressed():compressedImage,
+						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
 						cameraModels,
 						id,
 						0,
@@ -5986,12 +6023,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),
@@ -6007,12 +6044,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData):
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.maxPoints(),
 									laserScan.rangeMax(),
 									laserScan.format(),
 									laserScan.localTransform()):
-								LaserScan(compressedScan,
+								LaserScan(compressedScan.empty()?data.laserScanCompressed().data():compressedScan,
 									laserScan.format(),
 									laserScan.rangeMin(),
 									laserScan.rangeMax(),

corelib/src/Odometry.cpp

@@ -337,6 +337,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	if((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
+	   (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
+	   (data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
+	{
+		UDEBUG("Received compressed data, uncompressing...");
+		data.uncompressData();
+		UDEBUG("Received compressed data, uncompressing...done!");
+	}
+
 	if(!data.imageRaw().empty())
 	{
 		UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",

corelib/src/OdometryThread.cpp

@@ -123,6 +123,9 @@ void OdometryThread::mainLoop()
 		{
 			UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
 			// a null pose notify that odometry could not be computed
+			data.setImageRaw(cv::Mat());
+			if(!data.depthOrRightCompressed().empty())
+				data.setDepthOrRightRaw(cv::Mat());
 			this->post(new OdometryEvent(data, pose, info));
 		}
 	}
@@ -155,7 +158,11 @@ void OdometryThread::addData(const SensorData & data)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || data.imu().empty())
+		if( !data.imageRaw().empty() ||
+			!data.imageCompressed().empty() ||
+			!data.laserScanRaw().isEmpty() ||
+			!data.laserScanCompressed().empty() || 
+			data.imu().empty())
 		{
 			_dataBuffer.push_back(data);
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)

corelib/src/SensorCaptureThread.cpp

@@ -530,7 +530,7 @@ void SensorCaptureThread::mainLoop()
 		info.odomPose.setNull();
 	}
 
-	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
+	if(!data.imageCompressed().empty() || !data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
 		info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();

corelib/src/SensorData.cpp

@@ -365,7 +365,8 @@ void SensorData::setStereoImage(
 	}
 	else if(!right.empty())
 	{
-		UASSERT(right.type() == CV_8UC1); // Mono
+		UASSERT(right.type() == CV_8UC1 || // Mono
+				right.type() == CV_8UC3);  // RGB
 		_depthOrRightRaw = right;
 		if(clearData)
 		{

corelib/src/Stereo.cpp

@@ -79,6 +79,8 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
 		const std::vector<cv::Point2f> & leftCorners,
 		std::vector<unsigned char> & status) const
 {
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	std::vector<cv::Point2f> rightCorners;
 	UDEBUG("util2d::calcStereoCorrespondences() begin");
 	rightCorners = util2d::calcStereoCorrespondences(
@@ -145,6 +147,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 		const std::vector<cv::Point2f> & leftCorners,
 		std::vector<unsigned char> & status) const
 {
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	std::vector<cv::Point2f> rightCorners;
 	std::vector<float> err;
 #ifdef HAVE_OPENCV_CUDAOPTFLOW
@@ -184,6 +188,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 {
 	std::vector<cv::Point2f> rightCorners;
 #ifdef HAVE_OPENCV_CUDAOPTFLOW
+	UASSERT(leftImage.type() == CV_8UC1);
+	UASSERT(rightImage.type() == CV_8UC1);
 	UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
 	cv::cuda::GpuMat d_leftImage(leftImage);
 	cv::cuda::GpuMat d_rightImage(rightImage);

corelib/src/camera/CameraStereoImages.cpp

@@ -44,7 +44,8 @@ CameraStereoImages::CameraStereoImages(
 		float imageRate,
 		const Transform & localTransform) :
 		CameraImages(pathLeftImages, imageRate, localTransform),
-		camera2_(new CameraImages(pathRightImages))
+		camera2_(new CameraImages(pathRightImages)),
+		rightGrayScale_(true)
 {
 	this->setImagesRectified(rectifyImages);
 }
@@ -55,7 +56,8 @@ CameraStereoImages::CameraStereoImages(
 		float imageRate,
 		const Transform & localTransform) :
 		CameraImages("", imageRate, localTransform),
-		camera2_(0)
+		camera2_(0),
+		rightGrayScale_(true)
 {
 	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
 	if(paths.size() >= 1)
@@ -179,7 +181,7 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
 			// Rectification
 			cv::Mat leftImage = left.imageRaw();
 			cv::Mat rightImage = right.imageRaw();
-			if(rightImage.type() != CV_8UC1)
+			if(rightImage.type() != CV_8UC1 && rightGrayScale_)
 			{
 				cv::Mat tmp;
 				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);

corelib/src/camera/CameraStereoVideo.cpp

@@ -56,7 +56,8 @@ CameraStereoVideo::CameraStereoVideo(
 		usbDevice_(0),
 		usbDevice2_(-1),
 		_width(0),
-		_height(0)
+		_height(0),
+		rightGrayScale_(true)
 {
 }
 
@@ -74,7 +75,8 @@ CameraStereoVideo::CameraStereoVideo(
 		usbDevice_(0),
 		usbDevice2_(-1),
 		_width(0),
-		_height(0)
+		_height(0),
+		rightGrayScale_(true)
 {
 }
 
@@ -89,7 +91,8 @@ CameraStereoVideo::CameraStereoVideo(
 	usbDevice_(device),
 	usbDevice2_(-1),
 	_width(0),
-	_height(0)
+	_height(0),
+	rightGrayScale_(true)
 {
 }
 
@@ -105,7 +108,8 @@ CameraStereoVideo::CameraStereoVideo(
 	usbDevice_(deviceLeft),
 	usbDevice2_(deviceRight),
 	_width(0),
-	_height(0)
+	_height(0),
+	rightGrayScale_(true)
 {
 }
 
@@ -379,7 +383,7 @@ SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info)
 
 		// Rectification
 		bool rightCvt = false;
-		if(rightImage.type() != CV_8UC1)
+		if(rightImage.type() != CV_8UC1 && rightGrayScale_)
 		{
 			cv::Mat tmp;
 			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);

corelib/src/camera/CameraStereoZed.cpp

@@ -783,7 +783,10 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 #endif
 				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
-				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+				if(rightGrayScale_)
+					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+				else
+					cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2BGR);
 #if ZED_SDK_MAJOR_VERSION < 3
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 #else
@@ -891,4 +894,11 @@ void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
 	postInterIMU(imu, stamp);
 }
 
+void CameraStereoZed::setRightGrayScale(bool enabled)
+{
+#ifdef RTABMAP_ZED
+	rightGrayScale_ = enabled;
+#endif
+}
+
 } // namespace rtabmap

corelib/src/camera/CameraStereoZedOC.cpp

@@ -755,7 +755,10 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
 
 					// ----> Extract left and right images from side-by-side
 					left = frameBGR(cv::Rect(0, 0, frameBGR.cols / 2, frameBGR.rows));
-					cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
+					if(rightGrayScale_)
+						cv::cvtColor(frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows)),right,cv::COLOR_BGR2GRAY);
+					else
+						right = frameBGR(cv::Rect(frameBGR.cols / 2, 0, frameBGR.cols / 2, frameBGR.rows));
 					// <---- Extract left and right images from side-by-side
 
 					if(stereoModel_.isValidForRectification())
@@ -792,4 +795,11 @@ SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
 	return data;
 }
 
+void CameraStereoZedOC::setRightGrayScale(bool enabled)
+{
+#ifdef RTABMAP_ZEDOC
+	rightGrayScale_ = enabled;
+#endif
+}
+
 } // namespace rtabmap

corelib/src/odometry/OdometryMono.cpp

@@ -205,17 +205,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	int nFeatures = 0;
 
 	// convert to grayscale
-	if(data.imageRaw().channels() > 1)
+	if(data.imageRaw().channels() > 1 || data.rightRaw().channels() > 1)
 	{
-		cv::Mat newFrame;
-		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
+		cv::Mat newFrame = data.imageRaw();
+		cv::Mat newFrameRight = data.rightRaw();
+		if(data.imageRaw().channels() > 1)
+			cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
+		if(data.rightRaw().channels() > 1)
+			cv::cvtColor(data.rightRaw(), newFrameRight, cv::COLOR_BGR2GRAY);
 		if(!data.stereoCameraModels().empty())
 		{
-			data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModels());
+			data.setStereoImage(newFrame, newFrameRight, data.stereoCameraModels());
 		}
 		else
 		{
-			data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels());
+			data.setRGBDImage(newFrame, newFrameRight, data.cameraModels());
 		}
 	}
 

corelib/src/odometry/OdometryORBSLAM3.cpp

@@ -448,11 +448,16 @@ Transform OdometryORBSLAM3::computeTransform(
 	{
 		localTransform = data.stereoCameraModels()[0].localTransform();
 		cv::Mat leftMono = data.imageRaw();
-                if(data.imageRaw().channels() == 3) {
+        if(data.imageRaw().channels() == 3) {
 			leftMono = cv::Mat();
 			cv::cvtColor(data.imageRaw(), leftMono, CV_BGR2GRAY);
 		}
-		Tcw = orbslam_->TrackStereo(leftMono, data.rightRaw(), data.stamp(), orbslamImus_);
+		cv::Mat rightMono = data.rightRaw();
+        if(data.rightRaw().channels() == 3) {
+			rightMono = cv::Mat();
+			cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
+		}
+		Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
 		orbslamImus_.clear();
 	}
 	else

corelib/src/stereo/StereoBM.cpp

@@ -82,7 +82,8 @@ cv::Mat StereoBM::computeDisparity(
 {
 	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(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
+	UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -94,6 +95,16 @@ cv::Mat StereoBM::computeDisparity(
 		leftMono = leftImage;
 	}
 
+	cv::Mat rightMono;
+	if(rightImage.channels() == 3)
+	{
+		cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightImage;
+	}
+
 	cv::Mat disparity;
 #if CV_MAJOR_VERSION < 3
 	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
@@ -106,7 +117,7 @@ cv::Mat StereoBM::computeDisparity(
 	stereo.state->textureThreshold = textureThreshold_;
 	stereo.state->speckleWindowSize = speckleWindowSize_;
 	stereo.state->speckleRange = speckleRange_;
-	stereo(leftMono, rightImage, disparity, CV_16SC1);
+	stereo(leftMono, rightMono, disparity, CV_16SC1);
 #else
 	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
 	stereo->setBlockSize(blockSize_);
@@ -119,7 +130,7 @@ cv::Mat StereoBM::computeDisparity(
 	stereo->setSpeckleWindowSize(speckleWindowSize_);
 	stereo->setSpeckleRange(speckleRange_);
 	stereo->setDisp12MaxDiff(disp12MaxDiff_);
-	stereo->compute(leftMono, rightImage, disparity);
+	stereo->compute(leftMono, rightMono, disparity);
 #endif
 
 	if(minDisparity_>0)

corelib/src/stereo/StereoSGBM.cpp

@@ -71,7 +71,8 @@ cv::Mat StereoSGBM::computeDisparity(
 {
 	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(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3);
+	UASSERT(rightImage.type() == CV_8UC1 || rightImage.type() == CV_8UC3);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -83,6 +84,16 @@ cv::Mat StereoSGBM::computeDisparity(
 		leftMono = leftImage;
 	}
 
+	cv::Mat rightMono;
+	if(rightImage.channels() == 3)
+	{
+		cv::cvtColor(rightImage, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightImage;
+	}
+
 	cv::Mat disparity;
 #if CV_MAJOR_VERSION < 3
 	cv::StereoSGBM stereo(
@@ -97,7 +108,7 @@ cv::Mat StereoSGBM::computeDisparity(
 			speckleWindowSize_,
 			speckleRange_,
             mode_==1);
-	stereo(leftMono, rightImage, disparity);
+	stereo(leftMono, rightMono, disparity);
 #else
 	cv::Ptr<cv::StereoSGBM> stereo = cv::StereoSGBM::create(
 			minDisparity_,
@@ -111,7 +122,7 @@ cv::Mat StereoSGBM::computeDisparity(
 			speckleWindowSize_,
 			speckleRange_,
             mode_);
-	stereo->compute(leftMono, rightImage, disparity);
+	stereo->compute(leftMono, rightMono, disparity);
 #endif
 
 	if(minDisparity_>0)

corelib/src/util3d.cpp

@@ -822,14 +822,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
-	UASSERT(imageRight.type() == CV_8UC1);
+	UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3);
 	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
 	UASSERT(imageLeft.rows == imageRight.rows &&
 			imageLeft.cols == imageRight.cols);
 	UASSERT(decimation >= 1.0f);
 
 	cv::Mat leftColor = imageLeft;
-	cv::Mat rightMono = imageRight;
+	cv::Mat rightColor = imageRight;
 
 	cv::Mat leftMono;
 	if(leftColor.channels() == 3)
@@ -841,6 +841,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		leftMono = leftColor;
 	}
 
+	cv::Mat rightMono;
+	if(rightColor.channels() == 3)
+	{
+		cv::cvtColor(rightColor, rightMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		rightMono = rightColor;
+	}
+
 	return cloudFromDisparityRGB(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
@@ -954,7 +964,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
 	{
 		//stereo
-		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3);
 
 		cv::Mat leftMono;
 		if(sensorData.imageRaw().channels() == 3)
@@ -966,6 +976,16 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 
+		cv::Mat rightMono;
+		if(sensorData.rightRaw().channels() == 3)
+		{
+			cv::cvtColor(sensorData.rightRaw(), rightMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			rightMono = sensorData.rightRaw();
+		}
+
 		UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
 		UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
 		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
@@ -979,7 +999,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			if(sensorData.stereoCameraModels()[i].isValidForProjection())
 			{
 				cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
-				cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
+				cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
 				StereoCameraModel model = sensorData.stereoCameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||