CameraRealSense2: Added Dual Mode (T265+D400)

introlab · Oct 4, 2019 · 9cb1e4b · 9cb1e4b
1 parent 3ab5ec2
commit 9cb1e4b
Show file tree

Hide file tree

Showing 6 changed files with 228 additions and 66 deletions.
diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h
@@ -76,6 +76,7 @@ class RTABMAP_EXP CameraRealSense2 :
 	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
 	void setResolution(int width, int height, int fps = 30);
 	void publishInterIMU(bool enabled);
+	void setDualMode(bool enabled, const Transform & extrinsics);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
 	void setOdomProvided(bool enabled);
@@ -98,7 +99,7 @@ class RTABMAP_EXP CameraRealSense2 :
 private:
 #ifdef RTABMAP_REALSENSE2
 	rs2::context * ctx_;
-	rs2::device * dev_;
+	std::vector<rs2::device *> dev_;
 	std::string deviceId_;
 	rs2::syncer * syncer_;
 	float depth_scale_meters_;
@@ -128,6 +129,8 @@ class RTABMAP_EXP CameraRealSense2 :
 	int cameraHeight_;
 	int cameraFps_;
 	bool publishInterIMU_;
+	bool dualMode_;
+	Transform dualExtrinsics_;
 
 	static Transform realsense2PoseRotation_;
 	static Transform realsense2PoseRotationInv_;