Added resolution parameters for Usb camera and realsense2 sources

corelib/include/rtabmap/core/camera/CameraRealSense2.h

@@ -59,7 +59,6 @@ public:
 	static bool available();
 
 public:
-	// default local transform z in, x right, y down));
 	CameraRealSense2(
 		const std::string & deviceId = "",
 		float imageRate = 0,
@@ -72,8 +71,11 @@ public:
 	bool odomProvided() const;
 
 	// parameters are set during initialization
+	// D400 series
 	void setEmitterEnabled(bool enabled);
 	void setIRDepthFormat(bool enabled);
+	void setResolution(int width, int height, int fps = 30);
+	// T265 related parameters
 	void setImagesRectified(bool enabled);
 	void setOdomProvided(bool enabled);
 
@@ -117,6 +119,9 @@ private:
 	bool irDepth_;
 	bool rectifyImages_;
 	bool odometryProvided_;
+	int cameraWidth_;
+	int cameraHeight_;
+	int cameraFps_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;

corelib/include/rtabmap/core/camera/CameraVideo.h

@@ -58,6 +58,13 @@ public:
 	int getUsbDevice() const {return _usbDevice;}
 	const std::string & getFilePath() const {return _filePath;}
 
+	/**
+	 * Set wanted usb resolution, should be set before initialization. 0 means
+	 * default resolution. It won't be applied if a valid camera calibration
+	 * has been loaded, thus resolution from calibration is used.
+	 * */
+	void setResolution(int width, int height) {_width=width, _height=height;}
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
@@ -72,6 +79,8 @@ private:
 	// Usb camera
 	int _usbDevice;
 	std::string _guid;
+	int _width;
+	int _height;
 
 	CameraModel _model;
 };

corelib/src/camera/CameraRealSense2.cpp

@@ -67,7 +67,10 @@ CameraRealSense2::CameraRealSense2(
 	emitterEnabled_(true),
 	irDepth_(false),
 	rectifyImages_(true),
-	odometryProvided_(false)
+	odometryProvided_(false),
+	cameraWidth_(640),
+	cameraHeight_(480),
+	cameraFps_(30)
 #endif
 {
 	UDEBUG("");
@@ -564,21 +567,21 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			{
 				//D400 series:
 				if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
-					video_profile.width()  == 640 &&
-					video_profile.height() == 480 &&
-					video_profile.fps()    == 30)
+					video_profile.width()  == cameraWidth_ &&
+					video_profile.height() == cameraHeight_ &&
+					video_profile.fps()    == cameraFps_)
 				{
 					profilesPerSensor[irDepth_?1:i].push_back(profile);
 					auto intrinsic = video_profile.get_intrinsics();
 					if(i==1)
 					{
-						depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
+						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), CV_16UC1, cv::Scalar(0));
 						depthStreamProfile = profile;
 						*depthIntrinsics_ = intrinsic;
 					}
 					else
 					{
-						rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
+						rgbBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
 						model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
 						rgbStreamProfile = profile;
 						*rgbIntrinsics_ = intrinsic;
@@ -638,7 +641,17 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		if (!added)
 		{
 			UERROR("Given stream configuration is not supported by the device! "
-					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
+					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, cameraWidth_, cameraHeight_, cameraFps_);
+			UERROR("Available configurations:");
+			for (auto& profile : profiles)
+			{
+				auto video_profile = profile.as<rs2::video_stream_profile>();
+				UERROR("%s %d %d %d", rs2_format_to_string(
+						video_profile.format()),
+						video_profile.width(),
+						video_profile.height(),
+						video_profile.fps());
+			}
 			return false;
 		}
 	 }
@@ -819,6 +832,15 @@ void CameraRealSense2::setIRDepthFormat(bool enabled)
 #endif
 }
 
+void CameraRealSense2::setResolution(int width, int height, int fps)
+{
+#ifdef RTABMAP_REALSENSE2
+	cameraWidth_ = width;
+	cameraHeight_ = height;
+	cameraFps_ = fps;
+#endif
+}
+
 void CameraRealSense2::setImagesRectified(bool enabled)
 {
 #ifdef RTABMAP_REALSENSE2

corelib/src/camera/CameraVideo.cpp

@@ -43,7 +43,9 @@ CameraVideo::CameraVideo(
 	Camera(imageRate, localTransform),
 	_rectifyImages(rectifyImages),
 	_src(kUsbDevice),
-	_usbDevice(usbDevice)
+	_usbDevice(usbDevice),
+	_width(0),
+	_height(0)
 {
 
 }
@@ -57,7 +59,9 @@ CameraVideo::CameraVideo(
 	_filePath(filePath),
 	_rectifyImages(rectifyImages),
 	_src(kVideoFile),
-	_usbDevice(0)
+	_usbDevice(0),
+	_width(0),
+	_height(0)
 {
 }
 
@@ -123,6 +127,19 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 			}
 		}
 		_model.setLocalTransform(this->getLocalTransform());
+		if(_src == kUsbDevice)
+		{
+			if(_model.isValidForProjection())
+			{
+				_capture.set(cv::CAP_PROP_FRAME_WIDTH, _model.imageWidth());
+				_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
+			}
+			else if(_width > 0 && _height > 0)
+			{
+				_capture.set(cv::CAP_PROP_FRAME_WIDTH, _width);
+				_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _height);
+			}
+		}
 		if(_rectifyImages && !_model.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");