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