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

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