CID-SIMS support (#1676)

* cid-sims dataset support

* Added cli tool to test CID-SIMS dataset

* removed debug log

Author: matlabbe

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

@@ -102,10 +102,11 @@ class RTABMAP_CORE_EXPORT CameraImages :
 	}
 
 	// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	void setGroundTruthPath(const std::string & filePath, int format = 0, const Transform & localTransform = Transform::getIdentity())
 	{
 		_groundTruthPath = filePath;
 		_groundTruthFormat = format;
+		_groundTruthLocalTransform = localTransform;
 	}
 
 	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
@@ -164,6 +165,7 @@ class RTABMAP_CORE_EXPORT CameraImages :
 	int _odometryFormat;
 	std::string _groundTruthPath;
 	int _groundTruthFormat;
+	Transform _groundTruthLocalTransform;
 	double _maxPoseTimeDiff;
 
 	std::list<double> _stamps;

corelib/src/CameraModel.cpp

@@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath)
 						data[0], data[1], data[2], data[3],
 						data[4], data[5], data[6], data[7],
 						data[8], data[9], data[10], data[11]);
+				Transform detCheck = localTransform_.clone();
+				localTransform_.normalizeRotation(); /// Normalize by default
+				float det = detCheck.toEigen3f().linear().determinant();
+				if(fabs(det - 1.0f) > 0.0001)
+				{
+					std::stringstream streamBefore, streamAfter;
+					streamBefore << detCheck << std::endl;
+					streamAfter << localTransform_ << std::endl;
+					UWARN("The camera model's local_transform from \"%s\" doesn't "
+						"have a normalized rotation matrix (dertminant=%f). We will normalize "
+						"it for convenience.\nWas:\n%sNow\n%s",
+						filePath.c_str(),
+						det,
+						streamBefore.str().c_str(),
+						streamAfter.str().c_str());
+				}
 			}
 			else
 			{

corelib/src/IMUThread.cpp

@@ -135,8 +135,20 @@ void IMUThread::mainLoop()
 		std::stringstream stream(line);
 		std::string s;
 		std::getline(stream, s, ',');
-		std::string nanoseconds = s.substr(s.size() - 9, 9);
-		std::string seconds = s.substr(0, s.size() - 9);
+
+		double stamp = 0.0;
+		if(s.find('.') != std::string::npos)
+		{
+			// Normal [epoch] timestamp
+			stamp = uStr2Double(s);
+		}
+		else
+		{
+			// Assume EuRoC format
+			std::string nanoseconds = s.substr(s.size() - 9, 9);
+			std::string seconds = s.substr(0, s.size() - 9);
+			stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
+		}
 
 		cv::Vec3d gyr;
 		for (int j = 0; j < 3; ++j) {
@@ -150,7 +162,6 @@ void IMUThread::mainLoop()
 			acc[j] = uStr2Double(s);
 		}
 
-		double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
 		if(previousStamp_>0 && stamp > previousStamp_)
 		{
 			captureDelay_ = stamp - previousStamp_;

corelib/src/OdometryThread.cpp

@@ -188,7 +188,7 @@ void OdometryThread::addData(const SensorEvent & event)
 					"(%f), skipping that frame (imu buffer size=%ld). "
 					"When using async IMU, make sure IMU is published faster "
 					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar)."
-					"Current camera/lidar delay is %fs.",
+					"Current camera/lidar delay with system time is %fs.",
 					event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
 				notify = false;
 			}
@@ -197,7 +197,7 @@ void OdometryThread::addData(const SensorEvent & event)
 					"(%f), skipping that frame (imu buffer size=%ld). "
 					"When using async IMU, make sure IMU is published faster "
 					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar). "
-					"Current camera/lidar delay is %fs.",
+					"Current camera/lidar delay with system time is %fs.",
 					event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
 				notify = false;
 			}

corelib/src/Transform.cpp

@@ -52,6 +52,26 @@ Transform::Transform(
 			r11, r12, r13, o14,
 			r21, r22, r23, o24,
 			r31, r32, r33, o34);
+
+	if( r11>0.0f || r12>0.0f || r13>0.0f ||
+		r21>0.0f || r22>0.0f || r23>0.0f ||
+		r31>0.0f || r32>0.0f || r33>0.0f)
+	{
+		Eigen::Matrix3f m;
+		m << r11, r12, r13,
+			r21, r22, r23,
+			r31, r32, r33;
+		float d = m.determinant();
+		if(fabs(d-1.0f) > 0.0001)
+		{
+			UWARN("Created transform doesn't have normalized rotation. Any transformation with this transform can cause unexpected results!"
+				" Determinant([%f %f %f;%f %f %f;%f %f %f])=%f",
+				r11, r12, r13,
+				r21, r22, r23,
+				r31, r32, r33,
+				d);
+		}
+	}
 }
 
 Transform::Transform(const cv::Mat & transformationMatrix)
@@ -509,6 +529,11 @@ Transform Transform::fromString(const std::string & string)
 					  numbers[4], numbers[5], numbers[6], numbers[7],
 					  numbers[8], numbers[9], numbers[10], numbers[11]);
 	}
+	// Always normalize
+	if(!t.isNull())
+	{
+		t.normalizeRotation();
+	}
 	return t;
 }
 

corelib/src/camera/CameraImages.cpp

@@ -63,6 +63,7 @@ CameraImages::CameraImages() :
 		_syncImageRateWithStamps(true),
 		_odometryFormat(0),
 		_groundTruthFormat(0),
+		_groundTruthLocalTransform(Transform::getIdentity()),
 		_maxPoseTimeDiff(0.02),
 		_captureDelay(0.0)
 	{}
@@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path,
 	_syncImageRateWithStamps(true),
 	_odometryFormat(0),
 	_groundTruthFormat(0),
+	_groundTruthLocalTransform(Transform::getIdentity()),
 	_maxPoseTimeDiff(0.02),
 	_captureDelay(0.0)
 {
@@ -478,27 +480,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		if(success && _odometryPath.size() && odometry_.empty())
 		{
 			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
+			if(!success)
+			{
+				UERROR("Failed to read odometry poses.");
+			}
+			
+			if(success)
+			{
+				for(size_t i=0; i<odometry_.size(); ++i)
+				{
+					// linear cov = 0.0001
+					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
+					if(i!=0)
+					{
+						// angular cov = 0.000001
+						covariance.at<double>(3,3) *= 0.01;
+						covariance.at<double>(4,4) *= 0.01;
+						covariance.at<double>(5,5) *= 0.01;
+					}
+					covariances_.push_back(covariance);
+				}
+			}
 		}
 
 		if(success && _groundTruthPath.size())
 		{
 			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
-		}
-
-		if(!odometry_.empty())
-		{
-			for(size_t i=0; i<odometry_.size(); ++i)
+			if(!success)
 			{
-				// linear cov = 0.0001
-				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
-				if(i!=0)
+				UERROR("Failed to read ground truth poses.");
+			}
+			else if(!_groundTruthLocalTransform.isIdentity())
+			{
+				Transform gtInv = _groundTruthLocalTransform.inverse();
+				for(auto pose: groundTruth_)
 				{
-					// angular cov = 0.000001
-					covariance.at<double>(3,3) *= 0.01;
-					covariance.at<double>(4,4) *= 0.01;
-					covariance.at<double>(5,5) *= 0.01;
+					pose = pose*gtInv; // pose of base_link, assuming ground truth frame and base frame are rigidly fixed
 				}
-				covariances_.push_back(covariance);
 			}
 		}
 	}
@@ -607,7 +625,7 @@ bool CameraImages::readPoses(
 		}
 		if(validPoses != (int)inOutStamps.size())
 		{
-			UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
+			UWARN("%d/%ld valid poses of %ld stamps", validPoses, outputPoses.size(), inOutStamps.size());
 		}
 	}
 	else
@@ -756,32 +774,19 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 
 			if(_stamps.size())
 			{
-				stamp = _stamps.front();
-				_stamps.pop_front();
-				if(_stamps.size())
-				{
-					_captureDelay = _stamps.front() - stamp;
-				}
+				UERROR("stamps cannot be used when startAt < 0");
 			}
 			if(odometry_.size())
 			{
-				odometryPose = odometry_.front();
-				odometry_.pop_front();
-				if(covariances_.size())
-				{
-					covariance = covariances_.front();
-					covariances_.pop_front();
-				}
+				UERROR("odometry cannot be used when startAt < 0");
 			}
 			if(groundTruth_.size())
 			{
-				groundTruthPose = groundTruth_.front();
-				groundTruth_.pop_front();
+				UERROR("groundTruth cannot be used when startAt < 0");
 			}
 			if(_models.size() && !model.isValidForProjection())
 			{
-				model = _models.front();
-				_models.pop_front();
+				UERROR("models cannot be used when startAt < 0");
 			}
 		}
 		else
@@ -792,6 +797,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 			{
 				imageFilePath = _path + imageFileName;
 				scanFilePath = _scanPath + scanFileName;
+				size_t stampsSize = _stamps.size();
 				if(_stamps.size())
 				{
 					stamp = _stamps.front();
@@ -803,6 +809,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 				}
 				if(odometry_.size())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), 
+						uFormat("Stamps=%ld odometry=%ld", _stamps.size(), odometry_.size()).c_str());
 					odometryPose = odometry_.front();
 					odometry_.pop_front();
 					if(covariances_.size())
@@ -813,11 +821,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 				}
 				if(groundTruth_.size())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), 
+						uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
 					groundTruthPose = groundTruth_.front();
 					groundTruth_.pop_front();
 				}
 				if(_models.size() && !model.isValidForProjection())
 				{
+					UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
+						uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
 					model = _models.front();
 					_models.pop_front();
 				}
@@ -834,6 +846,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 
 					imageFilePath = _path + imageFileName;
 					scanFilePath = _scanPath + scanFileName;
+					size_t stampsSize = _stamps.size();
 					if(_stamps.size())
 					{
 						stamp = _stamps.front();
@@ -845,6 +858,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					}
 					if(odometry_.size())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(), 
+							uFormat("Stamps=%ld odometry=%ld", stampsSize, odometry_.size()).c_str());
 						odometryPose = odometry_.front();
 						odometry_.pop_front();
 						if(covariances_.size())
@@ -855,11 +870,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					}
 					if(groundTruth_.size())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(), 
+							uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
 						groundTruthPose = groundTruth_.front();
 						groundTruth_.pop_front();
 					}
 					if(_models.size() && !model.isValidForProjection())
 					{
+						UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
+							uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
 						model = _models.front();
 						_models.pop_front();
 					}

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -293,6 +293,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog
 	double getSourceScanForceGroundNormalsUp() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
+	Transform getGroundTruthLocalTransform() const; // directory images
 	Transform getIMULocalTransform() const; // directory images
 	QString getIMUPath() const;
 	int getIMURate() const;

guilib/src/MainWindow.cpp

@@ -3313,7 +3313,7 @@ void MainWindow::updateMapCloud(
 							{
 								std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
 								color = Qt::gray;
-								_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
+								_cloudViewer->addOrUpdateFrustum(gtFrustumId, mapToGt*_currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
 							}
 						}
 					}