Added Ground truth import GPS format. Alignement between estimated and ground truth poses is done with SVD.

corelib/include/rtabmap/core/Graph.h

@@ -53,7 +53,7 @@ bool RTABMAP_EXP exportPoses(
 
 bool RTABMAP_EXP importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, GPS (t,x,y)
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1

corelib/include/rtabmap/core/util3d_registration.h

@@ -45,6 +45,10 @@ int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::Co
 							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 							float maxDistance);
 
+Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD(
+	const pcl::PointCloud<pcl::PointXYZ> & cloud1,
+	const pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
 Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,

corelib/src/CameraRGB.cpp

@@ -293,19 +293,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
 				success = false;
 			}
-			else if(_groundTruthFormat != 1 && poses.size() != this->imagesCount())
+			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5) && poses.size() != this->imagesCount())
 			{
 				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
 						"the ground truth file path if you don't want to use it (current file path=%s).",
 						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
 				success = false;
 			}
-			else if(_groundTruthFormat == 1 && stamps_.size() == 0)
+			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5) && stamps_.size() == 0)
 			{
-				UERROR("When using rgbd-slam format for ground truth, images must have timestamps!");
+				UERROR("When using RGBD-SLAM and GPS formats for ground truth, images must have timestamps!");
 				success = false;
 			}
-			else if(_groundTruthFormat == 1)
+			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5)
 			{
 				UDEBUG("");
 				//Match ground truth values with images
@@ -317,10 +317,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				}
 				std::vector<double> values = uValues(stamps);
 
+				int validPoses = 0;
 				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
 				{
 					Transform pose; // null transform
 					std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
+					bool warned = false;
 					if(endIter != stampsToIds.end())
 					{
 						if(endIter->first == *ster)
@@ -335,21 +337,37 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 							double stampBeg = beginIter->first;
 							double stampEnd = endIter->first;
 							UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-							float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+							if(stampEnd - stampBeg > 10.0)
-							Transform & ta = poses.at(beginIter->second);
-							Transform & tb = poses.at(endIter->second);
-							if(!ta.isNull() && !tb.isNull())
 							{
-								pose = ta.interpolate(t, tb);
+								warned = true;
+								UDEBUG("Cannot interpolate ground truth pose for stamp %f between %f and %f (>10 sec)", 
+									*ster,
+									stampBeg,
+									stampEnd);
+							}
+							else
+							{
+								float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+								Transform & ta = poses.at(beginIter->second);
+								Transform & tb = poses.at(endIter->second);
+								if(!ta.isNull() && !tb.isNull())
+								{
+									++validPoses;
+									pose = ta.interpolate(t, tb);
+								}
 							}
 						}
 					}
-					if(pose.isNull())
+					if(pose.isNull() && !warned)
 					{
-						UWARN("Ground truth pose not found for stamp %f", *ster);
+						UDEBUG("Ground truth pose not found for stamp %f", *ster);
 					}
 					groundTruth_.push_back(pose);
 				}
+				if(validPoses != (int)stamps.size())
+				{
+					UWARN("%d valid ground truth poses of %d stamps", validPoses, (int)stamps_.size());
+				}
 			}
 			else
 			{

corelib/src/Graph.cpp

@@ -153,7 +153,7 @@ bool exportPoses(
 
 bool importPoses(
 		const std::string & filePath,
-		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=GPS (t,x,y)
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> * constraints, // optional for formats 3 and 4
 		std::map<int, double> * stamps) // optional for format 1
@@ -197,7 +197,49 @@ bool importPoses(
 				continue;
 			}
 
-			if(format == 1) // rgbd-slam format
+			if(format == 5) // GPS format
+			{
+				std::vector<std::string> strList = uListToVector(uSplit(str));
+				if(strList.size() ==  3 || strList.size() ==  4)
+				{
+					if( uIsNumber(uReplaceChar(strList[0], ' ', "")) && 
+						uIsNumber(uReplaceChar(strList[1], ' ', "")) && 
+						uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
+						(strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
+					{
+						double stamp = uStr2Double(uReplaceChar(strList[0], ' ', ""));
+						double x = uStr2Double(uReplaceChar(strList[1], ' ', ""));
+						double y = uStr2Double(uReplaceChar(strList[2], ' ', ""));
+
+						if(stamps)
+						{
+							stamps->insert(std::make_pair(id, stamp));
+						}
+						float yaw = 0.0f; 
+						if(strList.size()==4)
+						{
+							yaw = uStr2Double(uReplaceChar(strList[3], ' ', ""));
+						}
+						else if(uContains(poses, id-1))
+						{
+							// set yaw depending on successive poses
+							Transform & previousPose = poses.at(id-1);
+							yaw = atan2(y-previousPose.y(),x-previousPose.x());
+							previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
+						}
+						poses.insert(std::make_pair(id, Transform(x,y,0,0,0,yaw)));
+					}
+					else
+					{
+						UDEBUG("Not valid values detected:  \"%s\"", str.c_str());
+					}
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with GPS format (should have 3 values: stamp x y)", str.c_str());
+				}
+			}
+			else if(format == 1) // rgbd-slam format
 			{
 				std::list<std::string> strList = uSplit(str);
 				if(strList.size() ==  8)
@@ -210,21 +252,24 @@ bool importPoses(
 					{
 						UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
 					}
-					if(stamps)
+					else
 					{
-						stamps->insert(std::make_pair(id, stamp));
+						if(stamps)
+						{
+							stamps->insert(std::make_pair(id, stamp));
+						}
+						// we need to remove optical rotation
+						// z pointing front, x left, y down
+						Transform t( 0, 0, 1, 0,
+									-1, 0, 0, 0,
+									 0,-1, 0, 0);
+						pose = t * pose * t.inverse();
+						t = Transform( 0, 0, 1, 0,
+									   0, -1, 0, 0,
+									   1, 0, 0, 0);
+						pose = t*pose;
+						poses.insert(std::make_pair(id, pose));
 					}
-					// we need to remove optical rotation
-					// z pointing front, x left, y down
-					Transform t( 0, 0, 1, 0,
-								-1, 0, 0, 0,
-								 0,-1, 0, 0);
-					pose = t * pose * t.inverse();
-					t = Transform( 0, 0, 1, 0,
-								   0, -1, 0, 0,
-								   1, 0, 0, 0);
-					pose = t*pose;
-					poses.insert(std::make_pair(id, pose));
 				}
 				else
 				{

corelib/src/util3d_registration.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/registration/icp.h>
 #include <pcl/registration/transformation_estimation_2D.h>
+#include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/sample_consensus/sac_model_registration.h>
 #include <pcl/sample_consensus/ransac.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -43,6 +44,19 @@ namespace rtabmap
 namespace util3d
 {
 
+// Get transform from cloud2 to cloud1
+Transform transformFromXYZCorrespondencesSVD(
+	const pcl::PointCloud<pcl::PointXYZ> & cloud1,
+	const pcl::PointCloud<pcl::PointXYZ> & cloud2)
+{
+	pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
+
+	// Perform the alignment
+	Eigen::Matrix4f matrix;
+	svd.estimateRigidTransformation(cloud1, cloud2, matrix);
+	return Transform::fromEigen4f(matrix);
+}
+
 // Get transform from cloud2 to cloud1
 Transform transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,

guilib/src/MainWindow.cpp

@@ -2134,45 +2134,45 @@ Transform MainWindow::alignPosesToGroundTruth(
 		std::map<int, Transform> & poses,
 		const std::map<int, Transform> & groundTruth)
 {
+	UDEBUG("");
 	Transform t = Transform::getIdentity();
-	if(groundTruth.size())
+	if(groundTruth.size() && poses.size())
 	{
-		if(uContains(_preferencesDialog->getAllParameters(), Parameters::kRGBDOptimizeFromGraphEnd()))
+		unsigned int maxSize = poses.size()>groundTruth.size()?poses.size():groundTruth.size();
+		pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+		cloud1.resize(maxSize);
+		cloud2.resize(maxSize);
+		int oi = 0;
+		int idFirst = 0;
+		for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
 		{
-			bool optimizeFromGraphEnd = uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kRGBDOptimizeFromGraphEnd()));
+			std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
-			// Align poses to ground truth
+			if(iter2!=poses.end())
-			int rootId = 0;
-			if(!optimizeFromGraphEnd)
 			{
-				for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
+				if(oi==0)
 				{
-					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
+					idFirst = iter->first;
-					if(iter2!=poses.end())
-					{
-						rootId = iter->first;
-						break;
-					}
 				}
+				cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+				cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
 			}
-			else
+		}
+		if(oi>1)
+		{
+			cloud1.resize(oi);
+			cloud2.resize(oi);
+		
+			t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+		}
+		else if(oi==1)
+		{
+			t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
+		}
+		if(!t.isIdentity())
+		{
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
-				for(std::map<int, Transform>::const_reverse_iterator iter=groundTruth.rbegin(); iter!=groundTruth.rend(); ++iter)
+				iter->second = t * iter->second;
-				{
-					std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
-					if(iter2!=poses.end())
-					{
-						rootId = iter->first;
-						break;
-					}
-				}
-			}
-			if(rootId>0)
-			{
-				t = groundTruth.at(rootId) * poses.at(rootId).inverse();
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					iter->second = t * iter->second;
-				}
 			}
 		}
 		UDEBUG("t=%s", t.prettyPrint().c_str());

guilib/src/PreferencesDialog.cpp

@@ -164,6 +164,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
 #endif
 	}
+	_ui->comboBox_cameraImages_gtFormat->setItemData(4, 0, Qt::UserRole - 1);
 	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 	{
 		_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-946</y>
+            <y>-499</y>
-            <width>676</width>
+            <width>680</width>
-            <height>1919</height>
+            <height>1171</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3434,6 +3434,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <string>TORO</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>g2o</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>GPS</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="4" column="2">

utilite/include/rtabmap/utilite/UStl.h

@@ -625,7 +625,7 @@ inline bool uIsDigit(const char c)
 }
 
 /**
- * Check if a string is a interger number.
+ * Check if a string is a integer number.
  * @param str the string
  * @return true if the string is a integer number
  */
@@ -634,11 +634,31 @@ inline bool uIsInteger(const std::string & str)
 	bool isInteger = str.size()!=0;
 	for(unsigned int i=0; i<str.size() && isInteger; ++i)
 	{
-		isInteger = uIsDigit(str[i]);
+		isInteger = i==0&&str[i]=='-'||uIsDigit(str[i]);
 	}
 	return isInteger;
 }
 
+/**
+ * Check if a string is a number (integer or float).
+ * @param str the string
+ * @return true if the string is a number
+ */
+inline bool uIsNumber(const std::string & str)
+{
+	std::list<std::string> list = uSplit(str, '.');
+	if(list.size() == 1)
+	{
+		return uIsInteger(str);
+	}
+	else if(list.size() == 2)
+	{
+		return uIsInteger(list.front()) && uIsInteger(list.back());
+	}
+	return false;
+}
+
+
 /**
  * Split a string into number and character strings.
  * Example: