Odom sensor option (#726)

* Added Odom Sensor option

* Odom Sensor: Added pose time offset parameter

* Gui: fixed odom cloud not shown when rgb/depth doesn't have same size. Odom sensor: fixed rawImages set on non-stereo camera (because stereoRectify wrongly set to all cameras)

* Calibration: fixed rgb and depth suffixes not correctly set. OpenNI2: horizontal and vertical shifts can be also set for default calib.

* Fixed a warning

* OdomSensor: added option to use odom sensor output as ground truth (for comparison between odom sensor and rtabmap odom). Added scale factor option.

* FindG2O.cmake: updated path suffixes to find EXTERNAL csparse

* Pose3GravityFactor: fixed dllimport error on windows

* UI: Statistics panel not updated if not visible and keep stats in cache is unchecked. Added description to some timing debug logs in processStatistics().

* Removed vtkOutputWindow on Windows

* Odom Sensor: added zed sdk support

* bump version 0.20.11

corelib/include/rtabmap/core/Camera.h

@@ -58,6 +58,7 @@ public:
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
 	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}

corelib/include/rtabmap/core/CameraThread.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 
@@ -58,6 +59,22 @@ class RTABMAP_EXP CameraThread :
 public:
 	// ownership transferred
 	CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose
+	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
+	 */
+	CameraThread(Camera * camera,
+			Camera * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			bool odomAsGt = false,
+			const ParametersMap & parameters = ParametersMap());
+	CameraThread(Camera * camera,
+				float poseScaleFactor,
+				bool odomAsGt,
+				const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
@@ -96,8 +113,10 @@ public:
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
+	bool odomProvided() const;
 
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
 
 private:
 	virtual void mainLoopBegin();
@@ -106,6 +125,11 @@ private:
 
 private:
 	Camera * _camera;
+	Camera * _odomSensor;
+	Transform _extrinsicsOdomToCamera;
+	bool _odomAsGt;
+	double _poseTimeOffset;
+	float _poseScaleFactor;
 	bool _mirroring;
 	bool _stereoExposureCompensation;
 	bool _colorOnly;

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

@@ -89,6 +89,7 @@ private:
 	int depth_resolution_;
 	bool ir_;
 	double previousStamp_;
+	double timestampOffset_;
 	UTimer timer_;
 	Transform imuLocalTransform_;
 #endif

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

@@ -69,7 +69,8 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	bool odomProvided() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
 
 	// parameters are set during initialization
 	// D400 series
@@ -83,7 +84,7 @@ public:
 	void setJsonConfig(const std::string & json);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
-	void setOdomProvided(bool enabled);
+	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false);
 
 #ifdef RTABMAP_REALSENSE2
 private:
@@ -112,7 +113,6 @@ private:
 	float depth_scale_meters_;
 	rs2_intrinsics depthIntrinsics_;
 	rs2_intrinsics rgbIntrinsics_;
-	rs2_extrinsics depthToRGBExtrinsics_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;
@@ -132,6 +132,7 @@ private:
 	bool irDepth_;
 	bool rectifyImages_;
 	bool odometryProvided_;
+	bool odometryImagesDisabled_;
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;

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

@@ -78,6 +78,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
 
 	void publishInterIMU(bool enabled);