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
2026-09-02 09:30:25 +08:00 · 2021-05-23 17:21:12 -04:00
parent fe896260c5
commit 21dbeed4b7
25 changed files with 1532 additions and 728 deletions
--- a/corelib/src/CameraThread.cpp
+++ b/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));
--- a/corelib/src/Memory.cpp
+++ b/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());
--- a/corelib/src/Rtabmap.cpp
+++ b/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;
 			}

--- a/corelib/src/camera/CameraK4A.cpp
+++ b/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),
--- a/corelib/src/camera/CameraOpenNI2.cpp
+++ b/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());
 						}
--- a/corelib/src/camera/CameraRealSense2.cpp
+++ b/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
 }

--- a/corelib/src/camera/CameraStereoZed.cpp
+++ b/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;
--- a/corelib/src/optimizer/gtsam/GravityFactor.h
+++ b/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;