Refactoring of the cameraStereoImages and cameraRGBDImages classes (now inheriting from CameraImages) for easy setting of laser scan path, timestamps path and ground truth path.

Added graph::importPoses().
Can now have a ground truth published with SensorData (filled optionally by CameraImages classes).
Increased database closing time performance when the database is not saved.
Added ParametersToolBox widget in DatabaseViewer for core parameters (refactoring done to make easy access to all rtabmap parameters in DatabaseViewer).
Added Parameters::getType(key).
Added Transform::interpolate() to interpolate between two transforms (SLERP)
Updated pf_filter.m and added test_pf_filter.m MATLAB scripts (making easier to compare with a ground truth)
UPlot: can now save all curve data of a figure in one action (see right-click on legend area->"Copy all curve data to clipboard")

corelib/src/Graph.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/common/common.h>
 #include <set>
 #include <queue>
+#include <fstream>
 
 #include <rtabmap/core/OptimizerTORO.h>
 #include <rtabmap/core/OptimizerG2O.h>
@@ -54,6 +55,7 @@ bool exportPoses(
 		const std::map<int, double> & stamps, // required for format 1
 		bool g2oRobust) // optional for format 4
 {
+	UDEBUG("%s", filePath.c_str());
 	std::string tmpPath = filePath;
 	if(format==3) // TORO
 	{
@@ -149,6 +151,98 @@ bool exportPoses(
 	return false;
 }
 
+bool importPoses(
+		const std::string & filePath,
+		int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
+		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
+{
+	UDEBUG("%s", filePath.c_str());
+	if(format==3) // TORO
+	{
+		std::multimap<int, Link> constraintsTmp;
+		if(OptimizerTORO::loadGraph(filePath, poses, constraintsTmp))
+		{
+			if(constraints)
+			{
+				*constraints = constraintsTmp;
+			}
+			return true;
+		}
+		return false;
+	}
+	else if(format == 4) // g2o
+	{
+		std::multimap<int, Link> constraintsTmp;
+		UERROR("Cannot import from g2o format because it is not yet supported!");
+		return false;
+	}
+	else
+	{
+		std::ifstream file;
+		file.open(filePath.c_str(), std::ifstream::in);
+		if(!file.good())
+		{
+			return false;
+		}
+		int id=1;
+		while(file.good())
+		{
+			std::string str;
+			std::getline(file, str);
+
+			if(str.front() == '#' || str.empty())
+			{
+				continue;
+			}
+
+			if(format == 1) // rgbd-slam format
+			{
+				std::list<std::string> strList = uSplit(str);
+				if(strList.size() ==  8)
+				{
+					double stamp = uStr2Float(strList.front());
+					strList.pop_front();
+					str = uJoin(strList, " ");
+					Transform pose = Transform::fromString(str);
+					if(pose.isNull())
+					{
+						UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
+					}
+					if(stamps)
+					{
+						stamps->insert(std::make_pair(id, stamp));
+					}
+					poses.insert(std::make_pair(id, pose));
+				}
+				else
+				{
+					UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values: stamp x y z qw qx qy qz)", str.c_str());
+				}
+			}
+			else // default / KITTI format
+			{
+				Transform pose = Transform::fromString(str);
+				if(format == 2)
+				{
+					// for KITTI, 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();
+				}
+				poses.insert(std::make_pair(id, pose));
+			}
+			++id;
+		}
+		file.close();
+		return true;
+	}
+	return false;
+}
+
 
 ////////////////////////////////////////////
 // Graph utilities