Camera class: localTransform parameter should not have optical rotation anymore (it is added afterwards in the constructor). OdomSensor: the extrinsics should not contain optical rotation anymore. These 2 changes make it more convenient to set transfomation parameters in the UI. MainWindow::createCamera() now has odomSensor argument for convenience (for inherited classes to set both camera and odom sensor in the same function).

2026-09-03 01:50:24 +08:00 · 2021-05-26 13:40:46 -04:00
parent 21dbeed4b7
commit ada3e75005
34 changed files with 96 additions and 80 deletions
--- a/corelib/include/rtabmap/core/Camera.h
+++ b/corelib/include/rtabmap/core/Camera.h
@@ -73,9 +73,10 @@ protected:
 	/**
 	 * Constructor
 	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
+	 * @param imageRate the frame rate (Hz), 0 for fast as the camera can
+	 * @param localTransform the transform from base frame to camera frame (without optical rotation)
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());

 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
--- a/corelib/include/rtabmap/core/CameraThread.h
+++ b/corelib/include/rtabmap/core/CameraThread.h
@@ -72,7 +72,6 @@ public:
 			bool odomAsGt = false,
 			const ParametersMap & parameters = ParametersMap());
 	CameraThread(Camera * camera,
-				float poseScaleFactor,
 				bool odomAsGt,
 				const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraThread();
--- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h
+++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h
@@ -54,7 +54,7 @@ public:
 			const std::string & deviceSerial = "",
 			int resolution = 1, // 0=720p, 1=800p, 2=400p
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraDepthAI();

 	void setOutputDepth(bool enabled, int confidence = 200);
--- a/corelib/include/rtabmap/core/camera/CameraFreenect.h
+++ b/corelib/include/rtabmap/core/camera/CameraFreenect.h
@@ -53,7 +53,7 @@ public:
 	CameraFreenect(int deviceId= 0,
 					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
-					const Transform & localTransform = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraFreenect2.h
+++ b/corelib/include/rtabmap/core/camera/CameraFreenect2.h
@@ -65,7 +65,7 @@ public:
 	CameraFreenect2(int deviceId= 0,
 					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
-					const Transform & localTransform = CameraModel::opticalRotation(),
+					const Transform & localTransform = Transform::getIdentity(),
 					float minDepth = 0.3f,
 					float maxDepth = 12.0f,
 					bool bilateralFiltering = true,
--- a/corelib/include/rtabmap/core/camera/CameraImages.h
+++ b/corelib/include/rtabmap/core/camera/CameraImages.h
@@ -46,7 +46,7 @@ public:
 	CameraImages(
 			const std::string & path,
 			float imageRate = 0,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraImages();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraK4A.h
+++ b/corelib/include/rtabmap/core/camera/CameraK4A.h
@@ -51,10 +51,10 @@ public:
 public:
 	CameraK4A(int deviceId = 0,
 		float imageRate = 0.0f,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	CameraK4A(const std::string & fileName,
 		float imageRate = 0.0f,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraK4A();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraK4W2.h
+++ b/corelib/include/rtabmap/core/camera/CameraK4W2.h
@@ -66,7 +66,7 @@ public:
 	CameraK4W2(int deviceId = 0, // not used
 		Type type = kTypeDepth2ColorSD,
 		float imageRate = 0.0f,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraK4W2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraMyntEye.h
+++ b/corelib/include/rtabmap/core/camera/CameraMyntEye.h
@@ -52,7 +52,7 @@ public:
 	static bool available();

 public:
-	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraMyntEye();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h
+++ b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h
@@ -54,7 +54,7 @@ public:
 	CameraOpenNI2(const std::string & deviceId = "",
 					Type type = kTypeColorDepth,
 					float imageRate = 0,
-					const Transform & localTransform = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h
+++ b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h
@@ -45,7 +45,7 @@ public:
 public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
-					const Transform & localTransform = CameraModel::opticalRotation());
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNICV();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraOpenni.h
+++ b/corelib/include/rtabmap/core/camera/CameraOpenni.h
@@ -66,7 +66,7 @@ public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
 #ifdef RTABMAP_OPENNI
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
--- a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h
+++ b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h
@@ -44,7 +44,7 @@ public:
 			const std::string & pathDepthImages,
 			float depthScaleFactor = 1.0f,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRGBDImages();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraRealSense.h
+++ b/corelib/include/rtabmap/core/camera/CameraRealSense.h
@@ -64,7 +64,7 @@ public:
 		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
 		bool computeOdometry = false,
 		float imageRate = 0,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();

 	void setDepthScaledToRGBSize(bool enabled);
--- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h
+++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h
@@ -63,7 +63,7 @@ public:
 	CameraRealSense2(
 		const std::string & deviceId = "",
 		float imageRate = 0,
-		const Transform & localTransform = CameraModel::opticalRotation());
+		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -80,6 +80,11 @@ public:
 	void setDepthResolution(int width, int height, int fps = 30);
 	void setGlobalTimeSync(bool enabled);
 	void publishInterIMU(bool enabled);
+	/**
+	 * Dual mode (D400+T265 or L500+T265)
+	 * @param enabled enable dual mode
+	 * @param extrinsics the extrinsics between T265 pose frame (middle of the camera) to D400/L500 main camera (without optical rotation).
+	 */
 	void setDualMode(bool enabled, const Transform & extrinsics);
 	void setJsonConfig(const std::string & json);
 	// T265 related parameters
--- a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h
@@ -45,7 +45,7 @@ public:
 	static bool available();

 public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoDC1394();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h
@@ -47,7 +47,7 @@ public:
 	static bool available();

 public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = CameraModel::opticalRotation());
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoFlyCapture2();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h
@@ -49,12 +49,12 @@ public:
 			const std::string & pathRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoImages();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraStereoTara.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoTara.h
@@ -53,7 +53,7 @@ public:
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());

 	virtual ~CameraStereoTara();

--- a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h
@@ -46,24 +46,24 @@ public:
 			const std::string & pathSideBySide,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			const std::string & pathLeft,
 			const std::string & pathRight,
 			bool rectifyImages = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			int device,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			int deviceLeft,
 			int deviceRight,
 			bool rectifyImages = false,
 			float imageRate = 0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h
@@ -57,7 +57,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation(),
+			const Transform & localTransform = Transform::getIdentity(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
@@ -68,7 +68,7 @@ public:
 			int confidenceThr = 100,
 			bool computeOdometry = false,
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation(),
+			const Transform & localTransform = Transform::getIdentity(),
 			bool selfCalibration = true,
 			bool odomForce3DoF = false,
 			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
--- a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h
+++ b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h
@@ -57,7 +57,7 @@ public:
 			int deviceId,
 			int resolution = 3, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
 			float imageRate=0.0f,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoZedOC();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/include/rtabmap/core/camera/CameraVideo.h
+++ b/corelib/include/rtabmap/core/camera/CameraVideo.h
@@ -45,11 +45,11 @@ public:
 	CameraVideo(int usbDevice = 0,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	CameraVideo(const std::string & filePath,
 			bool rectifyImages = false,
 			float imageRate = 0,
-			const Transform & localTransform = CameraModel::opticalRotation());
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraVideo();

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
--- a/corelib/src/Camera.cpp
+++ b/corelib/src/Camera.cpp
@@ -45,7 +45,7 @@ namespace rtabmap

 Camera::Camera(float imageRate, const Transform & localTransform) :
 	_imageRate(imageRate),
-	_localTransform(localTransform),
+	_localTransform(localTransform*CameraModel::opticalRotation()),
 	_targetImageSize(0,0),
 	_frameRateTimer(new UTimer()),
 	_seq(0)
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -88,7 +88,7 @@ CameraThread::CameraThread(
 		const ParametersMap & parameters) :
 			_camera(camera),
 			_odomSensor(odomSensor),
-			_extrinsicsOdomToCamera(extrinsics),
+			_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
 			_odomAsGt(odomAsGt),
 			_poseTimeOffset(poseTimeOffset),
 			_poseScaleFactor(poseScaleFactor),
@@ -123,14 +123,13 @@ CameraThread::CameraThread(
 // ownership transferred
 CameraThread::CameraThread(
 		Camera * camera,
-		float poseScaleFactor,
 		bool odomAsGt,
 		const ParametersMap & parameters) :
 			_camera(camera),
 			_odomSensor(0),
 			_odomAsGt(odomAsGt),
 			_poseTimeOffset(0.0),
-			_poseScaleFactor(poseScaleFactor),
+			_poseScaleFactor(1.0f),
 			_mirroring(false),
 			_stereoExposureCompensation(false),
 			_colorOnly(false),
@@ -153,7 +152,6 @@ CameraThread::CameraThread(
 			_imuBaseFrameConversion(false)
 {
 	UASSERT(_camera != 0);
-	UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
 	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
 }

--- a/corelib/src/camera/CameraRealSense2.cpp
+++ b/corelib/src/camera/CameraRealSense2.cpp
@@ -1186,7 +1186,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 #ifdef RTABMAP_REALSENSE2
 	UASSERT(!enabled || !extrinsics.isNull());
 	dualMode_ = enabled;
-	dualExtrinsics_ =  extrinsics;
+	dualExtrinsics_ =  extrinsics*CameraModel::opticalRotation();
 	if(dualMode_)
 	{
 		odometryProvided_ = true;