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);
 

corelib/src/CameraThread.cpp

@@ -49,6 +49,10 @@ namespace rtabmap
 // ownership transferred
 CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_camera(camera),
+		_odomSensor(0),
+		_odomAsGt(false),
+		_poseTimeOffset(0.0),
+		_poseScaleFactor(1.0f),
 		_mirroring(false),
 		_stereoExposureCompensation(false),
 		_colorOnly(false),
@@ -73,10 +77,91 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 	UASSERT(_camera != 0);
 }
 
+// ownership transferred
+CameraThread::CameraThread(
+		Camera * camera,
+		Camera * odomSensor,
+		const Transform & extrinsics,
+		double poseTimeOffset,
+		float poseScaleFactor,
+		bool odomAsGt,
+		const ParametersMap & parameters) :
+			_camera(camera),
+			_odomSensor(odomSensor),
+			_extrinsicsOdomToCamera(extrinsics),
+			_odomAsGt(odomAsGt),
+			_poseTimeOffset(poseTimeOffset),
+			_poseScaleFactor(poseScaleFactor),
+			_mirroring(false),
+			_stereoExposureCompensation(false),
+			_colorOnly(false),
+			_imageDecimation(1),
+			_stereoToDepth(false),
+			_scanFromDepth(false),
+			_scanDownsampleStep(1),
+			_scanRangeMin(0.0f),
+			_scanRangeMax(0.0f),
+			_scanVoxelSize(0.0f),
+			_scanNormalsK(0),
+			_scanNormalsRadius(0.0f),
+			_scanForceGroundNormalsUp(false),
+			_stereoDense(StereoDense::create(parameters)),
+			_distortionModel(0),
+			_bilateralFiltering(false),
+			_bilateralSigmaS(10),
+			_bilateralSigmaR(0.1),
+			_imuFilter(0),
+			_imuBaseFrameConversion(false)
+{
+	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
+	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
+	UDEBUG("_poseTimeOffset        =%f", _poseTimeOffset);
+	UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
+	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+}
+
+// ownership transferred
+CameraThread::CameraThread(
+		Camera * camera,
+		float poseScaleFactor,
+		bool odomAsGt,
+		const ParametersMap & parameters) :
+			_camera(camera),
+			_odomSensor(0),
+			_odomAsGt(odomAsGt),
+			_poseTimeOffset(0.0),
+			_poseScaleFactor(poseScaleFactor),
+			_mirroring(false),
+			_stereoExposureCompensation(false),
+			_colorOnly(false),
+			_imageDecimation(1),
+			_stereoToDepth(false),
+			_scanFromDepth(false),
+			_scanDownsampleStep(1),
+			_scanRangeMin(0.0f),
+			_scanRangeMax(0.0f),
+			_scanVoxelSize(0.0f),
+			_scanNormalsK(0),
+			_scanNormalsRadius(0.0f),
+			_scanForceGroundNormalsUp(false),
+			_stereoDense(StereoDense::create(parameters)),
+			_distortionModel(0),
+			_bilateralFiltering(false),
+			_bilateralSigmaS(10),
+			_bilateralSigmaR(0.1),
+			_imuFilter(0),
+			_imuBaseFrameConversion(false)
+{
+	UASSERT(_camera != 0);
+	UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
+	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+}
+
 CameraThread::~CameraThread()
 {
 	join(true);
 	delete _camera;
+	delete _odomSensor;
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
@@ -164,6 +249,11 @@ void CameraThread::setScanParameters(
 	_scanForceGroundNormalsUp = groundNormalsUp;
 }
 
+bool CameraThread::odomProvided() const
+{
+	return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided()));
+}
+
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
@@ -176,10 +266,51 @@ void CameraThread::mainLoop()
 	CameraInfo info;
 	SensorData data = _camera->takeImage(&info);
 
+	if(_odomSensor)
+	{
+		Transform pose;
+		Transform poseToLeftCam;
+		cv::Mat covariance;
+		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance))
+		{
+			info.odomPose = pose;
+			info.odomCovariance = covariance;
+			if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
+			{
+				info.odomPose.x() *= _poseScaleFactor;
+				info.odomPose.y() *= _poseScaleFactor;
+				info.odomPose.z() *= _poseScaleFactor;
+			}
+			// Adjust local transform of the camera based on the pose frame
+			if(!data.cameraModels().empty())
+			{
+				UASSERT(data.cameraModels().size()==1);
+				CameraModel model = data.cameraModels()[0];
+				model.setLocalTransform(_extrinsicsOdomToCamera);
+				data.setCameraModel(model);
+			}
+			else
+			{
+				StereoCameraModel model = data.stereoCameraModel();
+				model.setLocalTransform(_extrinsicsOdomToCamera);
+				data.setStereoCameraModel(model);
+			}
+		}
+		else
+		{
+			UWARN("Could not get pose at stamp %f", data.stamp());
+		}
+	}
+
+	if(_odomAsGt && !info.odomPose.isNull())
+	{
+		data.setGroundTruth(info.odomPose);
+		info.odomPose.setNull();
+	}
+
 	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
-
 		info.cameraName = _camera->getSerial();
 		info.timeTotal = totalTime.ticks();
 		this->post(new CameraEvent(data, info));

corelib/src/Memory.cpp

@@ -5260,6 +5260,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setLaserScan(laserScan, false);
 	s->sensorData().setUserData(data.userDataRaw(), false);
 
+	UDEBUG("data.groundTruth()      =%s", data.groundTruth().prettyPrint().c_str());
+	UDEBUG("data.gps()              =%s", data.gps().stamp()?"true":"false");
+	UDEBUG("data.envSensors()       =%d", (int)data.envSensors().size());
+	UDEBUG("data.globalDescriptors()=%d", (int)data.globalDescriptors().size());
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());

corelib/src/Rtabmap.cpp

@@ -4210,7 +4210,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, T
 			}
 			else
 			{
-				UWARN(uFormat("path.size()=0!? nearestId=%d ids=%d, aborting...", (int)path.size(), nearestId, (int)ids.size()).c_str());
+				UWARN("path.size()=0!? nearestId=%d ids=%d, aborting...", nearestId, (int)ids.size());
 				break;
 			}
 

corelib/src/camera/CameraK4A.cpp

@@ -65,7 +65,8 @@ CameraK4A::CameraK4A(
 			framerate_(2),
 			depth_resolution_(2),
 			ir_(false),
-			previousStamp_(0.0)
+			previousStamp_(0.0),
+			timestampOffset_(0.0)
 #endif
 {
 }
@@ -87,7 +88,8 @@ CameraK4A::CameraK4A(
 			framerate_(2),
 			depth_resolution_(2),
 			ir_(false),
-			previousStamp_(0.0)
+			previousStamp_(0.0),
+			timestampOffset_(0.0)
 #endif
 {
 }
@@ -120,6 +122,8 @@ void CameraK4A::close()
 		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
 		transformationHandle_ = NULL;
 	}
+	previousStamp_ = 0.0;
+	timestampOffset_ = 0.0;
 #endif
 }
 
@@ -504,7 +508,14 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 
 			if (depth_image_ != NULL)
 			{
-				stamp = ((double)k4a_image_get_timestamp_usec(depth_image_)) / 1000000;
+				double stampDevice = ((double)k4a_image_get_device_timestamp_usec(depth_image_)) / 1000000.0;
+
+				if(timestampOffset_ == 0.0)
+				{
+					timestampOffset_ = stamp - stampDevice;
+				}
+				if(!playbackHandle_)
+					stamp = stampDevice + timestampOffset_;
 
 				if (ir_)
 				{
@@ -553,7 +564,6 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 			k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN);
 			if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_))
 			{
-				double stmp = ((double)imu_sample_.acc_timestamp_usec) / 1000000;
 				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
 						cv::Mat::eye(3, 3, CV_64FC1),
 						cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),

corelib/src/camera/CameraOpenNI2.cpp

@@ -529,6 +529,13 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 
 				if(_type==kTypeColorDepth)
 				{
+					if (_depthHShift > 0 || _depthVShift > 0)
+					{
+						cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
+						depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
+						depth = out;
+					}
+
 					if(_stereoModel.right().isValidForRectification())
 					{
 						rgb = _stereoModel.right().rectifyImage(rgb);
@@ -536,12 +543,6 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 
 						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
 						{
-							if (_depthHShift > 0 || _depthVShift > 0)
-							{
-								cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
-								depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
-								depth = out;
-							}
 							depth = _stereoModel.left().rectifyImage(depth, 0);
 							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
 						}

corelib/src/camera/CameraRealSense2.cpp

@@ -68,6 +68,7 @@ CameraRealSense2::CameraRealSense2(
 	irDepth_(true),
 	rectifyImages_(true),
 	odometryProvided_(false),
+	odometryImagesDisabled_(false),
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
@@ -231,10 +232,6 @@ void CameraRealSense2::getPoseAndIMU(
 	pose.setNull();
 	imu = IMU();
 	poseConfidence = 0;
-	if(accBuffer_.empty() || gyroBuffer_.empty())
-	{
-		return;
-	}
 
 	// Interpolate pose
 	if(!poseBuffer_.empty())
@@ -252,7 +249,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs > 0)
 			{
-				UWARN("Could not find poses to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+				UWARN("Could not find poses to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, poseBuffer_.rbegin()->first/1000.0);
 			}
 		}
 		else
@@ -283,11 +280,11 @@ void CameraRealSense2::getPoseAndIMU(
 				{
 					if(stamp < iterA->first)
 					{
-						UWARN("Could not find pose data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+						UWARN("Could not find pose data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
 					}
 					else
 					{
-						UWARN("Could not find pose data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+						UWARN("Could not find pose data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
 					}
 				}
 				if(!globalTimeSync_)
@@ -306,6 +303,11 @@ void CameraRealSense2::getPoseAndIMU(
 		poseMutex_.unlock();
 	}
 
+	if(accBuffer_.empty() || gyroBuffer_.empty())
+	{
+		return;
+	}
+
 	// Interpolate acc
 	cv::Vec3d acc;
 	{
@@ -325,7 +327,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find acc data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
+				UWARN("Could not find acc data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, accBuffer_.rbegin()->first/1000.0);
 			}
 			imuMutex_.unlock();
 			return;
@@ -361,11 +363,11 @@ void CameraRealSense2::getPoseAndIMU(
 				{
 					if(stamp < iterA->first)
 					{
-						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+						UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
 					}
 					else
 					{
-						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+						UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
 					}
 				}
 				if(!globalTimeSync_)
@@ -409,7 +411,7 @@ void CameraRealSense2::getPoseAndIMU(
 		{
 			if(maxWaitTimeMs>0)
 			{
-				UWARN("Could not find gyro data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
+				UWARN("Could not find gyro data to interpolate at image time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, gyroBuffer_.rbegin()->first/1000.0);
 			}
 			imuMutex_.unlock();
 			return;
@@ -445,11 +447,11 @@ void CameraRealSense2::getPoseAndIMU(
 				{
 					if(stamp < iterA->first)
 					{
-						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+						UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0);
 					}
 					else
 					{
-						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp/1000.0, iterA->first/1000.0, iterB->first/1000.0);
 					}
 				}
 				if(!globalTimeSync_)
@@ -882,25 +884,13 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 std::cout<< model_ << std::endl;
 			 return false;
 		 }
-		 depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
 
 		 if(dualMode_)
 		 {
-			 Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			 UINFO("Set base to pose");
-			 this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
-			 UINFO("poseToLeftIR = %s", dualExtrinsics_.prettyPrint().c_str());
-			 Transform baseToCam = this->getLocalTransform()*dualExtrinsics_*opticalTransform;
-			 if(!ir_)
-			 {
-				 Transform leftIRToRGB(
-						 depthToRGBExtrinsics_.rotation[0], depthToRGBExtrinsics_.rotation[1], depthToRGBExtrinsics_.rotation[2], depthToRGBExtrinsics_.translation[0],
-						 depthToRGBExtrinsics_.rotation[3], depthToRGBExtrinsics_.rotation[4], depthToRGBExtrinsics_.rotation[5], depthToRGBExtrinsics_.translation[1],
-						 depthToRGBExtrinsics_.rotation[6], depthToRGBExtrinsics_.rotation[7], depthToRGBExtrinsics_.rotation[8], depthToRGBExtrinsics_.translation[2]);
-				 leftIRToRGB = leftIRToRGB.inverse();
-				 UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str());
-				 baseToCam *= leftIRToRGB;
-			 }
+			 this->setLocalTransform(this->getLocalTransform()*CameraModel::opticalRotation().inverse());
+			 UINFO("dualExtrinsics_ = %s", dualExtrinsics_.prettyPrint().c_str());
+			 Transform baseToCam = this->getLocalTransform()*dualExtrinsics_;
 			 UASSERT(profilesPerSensor.size()>=2);
 			 UASSERT(profilesPerSensor.back().size() == 3);
 			 rs2_extrinsics poseToIMU = profilesPerSensor.back()[0].get_extrinsics_to(profilesPerSensor.back()[2]);
@@ -1012,11 +1002,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
 			UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
 
-			UINFO("Set base to pose");
 			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
 			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
 			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
+
+			if(odometryImagesDisabled_)
+			{
+				// keep only pose stream
+				std::vector<rs2::stream_profile> profiles;
+				profiles.push_back(profilesPerSensor[0][4]);
+				profilesPerSensor[0] = profiles;
+			}
 		}
 		else
 		{
@@ -1114,6 +1111,29 @@ bool CameraRealSense2::odomProvided() const
 #endif
 }
 
+bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance)
+{
+#ifdef RTABMAP_REALSENSE2
+	IMU imu;
+	unsigned int confidence = 0;
+	double rsStamp = stamp*1000.0;
+	Transform p;
+	getPoseAndIMU(rsStamp, p, confidence, imu);
+
+	if(!p.isNull())
+	{
+		// Transform in base frame
+		pose = this->getLocalTransform() * p * this->getLocalTransform().inverse();
+
+		covariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
+		covariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
+		covariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
+		return true;
+	}
+#endif
+	return false;
+}
+
 void CameraRealSense2::setEmitterEnabled(bool enabled)
 {
 #ifdef RTABMAP_REALSENSE2
@@ -1170,6 +1190,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 	if(dualMode_)
 	{
 		odometryProvided_ = true;
+		odometryImagesDisabled_ = false;
 	}
 #endif
 }
@@ -1188,7 +1209,7 @@ void CameraRealSense2::setImagesRectified(bool enabled)
 #endif
 }
 
-void CameraRealSense2::setOdomProvided(bool enabled)
+void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
 {
 #ifdef RTABMAP_REALSENSE2
 	if(dualMode_ && !enabled)
@@ -1197,6 +1218,7 @@ void CameraRealSense2::setOdomProvided(bool enabled)
 		dualMode_ = false;
 	}
 	odometryProvided_ = enabled;
+	odometryImagesDisabled_ = enabled && imageStreamsDisabled;
 #endif
 }
 

corelib/src/camera/CameraStereoZed.cpp

@@ -541,6 +541,82 @@ bool CameraStereoZed::odomProvided() const
 #endif
 }
 
+bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance)
+{
+#ifdef RTABMAP_ZED
+
+	if (computeOdometry_ && zed_)
+	{
+		sl::Pose p;
+
+#if ZED_SDK_MAJOR_VERSION < 3
+		if(!zed_->grab())
+		{
+			return false;
+		}
+		sl::TRACKING_STATE tracking_state = zed_->getPosition(p);
+		if (tracking_state == sl::TRACKING_STATE_OK)
+#else
+		if(zed_->grab()!=sl::ERROR_CODE::SUCCESS)
+		{
+			return false;
+		}
+		 sl::POSITIONAL_TRACKING_STATE tracking_state = zed_->getPosition(p);
+		if (tracking_state == sl::POSITIONAL_TRACKING_STATE::OK)
+#endif
+		{
+			int trackingConfidence = p.pose_confidence;
+			// FIXME What does pose_confidence == -1 mean?
+			if (trackingConfidence>0)
+			{
+				pose = zedPoseToTransform(p);
+				if (!pose.isNull())
+				{
+					//transform from:
+					// x->right, y->down, z->forward
+					//to:
+					// x->forward, y->left, z->up
+					pose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
+					if(force3DoF_)
+					{
+						pose = pose.to3DoF();
+					}
+					if (lost_)
+					{
+						covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+						lost_ = false;
+						UDEBUG("Init %s (var=%f)", pose.prettyPrint().c_str(), 9999.0f);
+					}
+					else
+					{
+						covariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+						UDEBUG("Run %s (var=%f)", pose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+					}
+					return true;
+				}
+				else
+				{
+					covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+					lost_ = true;
+					UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+				}
+			}
+			else
+			{
+				covariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+				lost_ = true;
+				UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+			}
+		}
+		else
+		{
+			UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
+		}
+	}
+#endif
+	return false;
+}
+
 SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;

corelib/src/optimizer/gtsam/GravityFactor.h

@@ -78,7 +78,7 @@ public:
  * Version of GravityFactor for Rot3
  * @addtogroup Navigation
  */
-class GTSAM_EXPORT Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
+class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
 
   typedef NoiseModelFactor1<Rot3> Base;
 
@@ -155,7 +155,7 @@ public:
  * Version of GravityFactor for Pose3
  * @addtogroup Navigation
  */
-class GTSAM_EXPORT Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
+class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
     public GravityFactor {
 
   typedef NoiseModelFactor1<Pose3> Base;