RealSense: added depthScaledToRGBSize option to be able to feed ORB_SLAM2 with depth and RGB images with same size #287.

corelib/include/rtabmap/core/CameraRGBD.h

@@ -386,6 +386,7 @@ public:
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();
 
+	void setDepthScaledToRGBSize(bool enabled) {depthScaledToRGBSize_ = enabled;}
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -402,6 +403,7 @@ private:
 	int presetRGB_;
 	int presetDepth_;
 	bool computeOdometry_;
+	bool depthScaledToRGBSize_;
 
 	int motionSeq_[2];
 	rs::slam::slam * slam_;
@@ -414,7 +416,7 @@ private:
 #endif
 };
 /////////////////////////
-// CameraRealSense
+// CameraRealSense2
 /////////////////////////
 class slam_event_handler;
 class RTABMAP_EXP CameraRealSense2 :

corelib/src/CameraRGBD.cpp

@@ -2623,6 +2623,7 @@ CameraRealSense::CameraRealSense(
 	presetRGB_(presetRGB),
 	presetDepth_(presetDepth),
 	computeOdometry_(computeOdometry),
+	depthScaledToRGBSize_(false),
 	slam_(0)
 #endif
 {
@@ -3132,7 +3133,8 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 		cv::Mat depth;
 		if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
 			depth_intrin.width < color_intrin.width &&
-			depth_intrin.height < color_intrin.height)
+			depth_intrin.height < color_intrin.height &&
+			!depthScaledToRGBSize_)
 		{
 			//we can keep the depth image size as is
 			depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);

corelib/src/OdometryORBSLAM2.cpp

@@ -859,7 +859,8 @@ Transform OdometryORBSLAM2::computeTransform(
 		data.imageRaw().rows != data.depthOrRightRaw().rows ||
 		data.imageRaw().cols != data.depthOrRightRaw().cols)
 	{
-		UERROR("Not supported input!");
+		UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
+				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
 		return t;
 	}