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/include/rtabmap/core/CameraRGB.h

@@ -49,23 +49,11 @@ class RTABMAP_EXP CameraImages :
 	public Camera
 {
 public:
-	CameraImages(const std::string & path,
-			int startAt = 1,
-			bool refreshDir = false,
-			bool rectifyImages = false,
-			bool isDepth = false,
+	CameraImages();
+	CameraImages(
+			const std::string & path,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
-	CameraImages(const std::string & scanPath,
-				const Transform & scanLocalTransform,
-				int scanMaxPts,
-			    const std::string & path,
-				int startAt = 1,
-				bool refreshDir = false,
-				bool rectifyImages = false,
-				bool isDepth = false,
-				float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraImages();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -74,6 +62,41 @@ public:
 	std::string getPath() const {return _path;}
 	unsigned int imagesCount() const;
 	std::vector<std::string> filenames() const;
+	bool isImagesRectified() const {return _rectifyImages;}
+	const CameraModel & cameraModel() const {return _model;}
+
+	void setPath(const std::string & dir) {_path=dir;}
+	void setStartIndex(int index) {_startAt = index;} // negative means last
+	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
+	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
+
+	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "")
+	{
+		_filenamesAreTimestamps = fileNamesAreStamps;
+		timestampsPath_=filePath;
+	}
+
+	void setScanPath(
+			const std::string & dir,
+			int maxScanPts = 0,
+			const Transform & localTransform=Transform::getIdentity())
+	{
+		_scanPath = dir;
+		_scanLocalTransform = localTransform;
+		_scanMaxPts = maxScanPts;
+	}
+
+	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	{
+		groundTruthPath_ = filePath;
+		_groundTruthFormat = format;
+	}
+
+	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
+	{
+		_isDepth = isDepth;
+		_depthScaleFactor=depthScaleFactor;
+	}
 
 protected:
 	virtual SensorData captureImage();
@@ -86,9 +109,11 @@ private:
 	bool _refreshDir;
 	bool _rectifyImages;
 	bool _isDepth;
+	float _depthScaleFactor;
 	int _count;
 	UDirectory * _dir;
 	std::string _lastFileName;
+
 	int _countScan;
 	UDirectory * _scanDir;
 	std::string _lastScanFileName;
@@ -96,7 +121,14 @@ private:
 	Transform _scanLocalTransform;
 	int _scanMaxPts;
 
-	std::string _cameraName;
+	bool _filenamesAreTimestamps;
+	std::string timestampsPath_;
+
+	std::string groundTruthPath_;
+	int _groundTruthFormat;
+
+	std::list<double> stamps_;
+	std::list<Transform> groundTruth_;
 	CameraModel _model;
 };
 

corelib/include/rtabmap/core/CameraRGBD.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/USemaphore.h"
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
@@ -267,7 +268,7 @@ private:
 /////////////////////////
 class CameraImages;
 class RTABMAP_EXP CameraRGBDImages :
-	public Camera
+	public CameraImages
 {
 public:
 	static bool available();
@@ -276,9 +277,7 @@ public:
 	CameraRGBDImages(
 			const std::string & pathRGBImages,
 			const std::string & pathDepthImages,
-			double depthScaleFactor = 1.0,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
+			float depthScaleFactor = 1.0f,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRGBDImages();
@@ -291,14 +290,7 @@ protected:
 	virtual SensorData captureImage();
 
 private:
-	CameraImages * cameraRGB_;
-	CameraImages * cameraDepth_;
-	double depthScaleFactor_;
-	bool filenamesAreTimestamps_;
-	std::string timestampsPath_;
-	std::list<double> stamps_;
-	CameraModel cameraModel_;
-	std::string cameraName_;
+	CameraImages cameraDepth_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraModel.h"
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include <list>
 
 namespace FlyCapture2
@@ -98,7 +99,7 @@ private:
 /////////////////////////
 class CameraImages;
 class RTABMAP_EXP CameraStereoImages :
-	public Camera
+	public CameraImages
 {
 public:
 	static bool available();
@@ -107,26 +108,11 @@ public:
 	CameraStereoImages(
 			const std::string & pathLeftImages,
 			const std::string & pathRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoImages(
 			const std::string & pathLeftRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & scanPath,
-			const Transform & scanLocalTransform,
-			int scanMaxPts,
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool filenamesAreTimestamps = false,
-			const std::string & timestampsPath = "", // "times.txt"
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
@@ -140,14 +126,8 @@ protected:
 	virtual SensorData captureImage();
 
 private:
-	CameraImages * camera_;
 	CameraImages * camera2_;
-	bool filenamesAreTimestamps_;
-	std::string timestampsPath_;
-	bool rectifyImages_;
-	std::list<double> stamps_;
 	StereoCameraModel stereoModel_;
-	std::string cameraName_;
 };
 
 

corelib/include/rtabmap/core/Features2d.h

@@ -102,7 +102,9 @@ public:
 		kFeatureGfttOrb=8,   //new 0.10.11
 		kFeatureFastOrb=9};  //new 0.11.0
 
-	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters);
+	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
+	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+	static Feature2D * create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters = ParametersMap()); // for convenience
 
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,

corelib/include/rtabmap/core/Graph.h

@@ -51,6 +51,13 @@ bool RTABMAP_EXP exportPoses(
 		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
 		bool g2oRobust = false); // optional for format 4
 
+bool RTABMAP_EXP 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 = 0, // optional for formats 3 and 4
+		std::map<int, double> * stamps = 0); // optional for format 1
+
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,

corelib/include/rtabmap/core/Memory.h

@@ -78,6 +78,7 @@ public:
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
 			bool postInitClosingEvents = false);
+	void close(bool databaseSaved = true, bool postInitClosingEvents = false);
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId(std::map<int, int> * reducedIds = 0);
@@ -253,7 +254,6 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
-	bool _postInitClosingEvents;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id

corelib/include/rtabmap/core/Odometry.h

@@ -100,6 +100,7 @@ private:
 	int _resetCurrentCount;
 	double previousStamp_;
 	Transform previousTransform_;
+	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
 
 	std::vector<ParticleFilter *> filters_;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -61,6 +61,7 @@ public:
 	double interval;
 	Transform transform;
 	Transform transformFiltered;
+	Transform transformGroundTruth;
 	float distanceTravelled;
 
 	int type; // 0=BOW, 1=Optical Flow, 2=ICP

corelib/include/rtabmap/core/Parameters.h

@@ -62,10 +62,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+		static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -94,10 +96,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+		static std::string type##PREFIX##NAME() {return std::string("string");} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, "string")); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -125,10 +129,12 @@ typedef std::pair<std::string, std::string> ParametersPair;
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static TYPE default##PREFIX##NAME() {return COND?DEFAULT_VALUE1:DEFAULT_VALUE2;} \
+		static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
 			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, COND?#DEFAULT_VALUE1:#DEFAULT_VALUE2)); \
+								   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
 								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
@@ -427,6 +433,12 @@ public:
 	{
 		return parameters_;
 	}
+	
+	/**
+	 * Get parameter type
+	 *
+	 */
+	static std::string getType(const std::string & paramKey);
 
 	/**
 	 * Get parameter description
@@ -469,6 +481,7 @@ private:
 
 private:
 	static ParametersMap parameters_;
+	static ParametersMap parametersType_;
 	static ParametersMap descriptions_;
 	static Parameters instance_;
 	

corelib/include/rtabmap/core/Registration.h

@@ -49,6 +49,21 @@ public:
 			int * inliersOut = 0,
 			float * varianceOut = 0,
 			float * inliersRatioOut = 0) = 0;
+
+	Transform computeTransformation2(
+				const SensorData & fromData,
+				const SensorData & toData,
+				Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
+				std::string * rejectedMsg = 0,
+				int * inliersOut = 0,
+				float * varianceOut = 0,
+				float * inliersRatioOut = 0)
+	{
+		Signature fromSignature(fromData.id(), -1, 0, 0.0, "", Transform::getIdentity(), fromData);
+		Signature toSignature(toData.id(), -1, 0, 0.0, "", Transform::getIdentity(), toData);
+		return computeTransformation(fromSignature, toSignature, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
+	}
+
 protected:
 	Registration(const ParametersMap & parameters = ParametersMap()) :
 		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())

corelib/include/rtabmap/core/Rtabmap.h

@@ -67,7 +67,7 @@ public:
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
 
-	void close();
+	void close(bool databaseSaved = true);
 
 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -61,7 +61,7 @@ public:
 	enum Cmd {
 			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
-			kCmdClose,
+			kCmdClose,            // params: [bool] database saved (default true)
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
 			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin

corelib/include/rtabmap/core/SensorData.h

@@ -191,6 +191,9 @@ public:
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
+	const Transform & groundTruth() const {return groundTruth_;}
+
 private:
 	int _id;
 	double _stamp;
@@ -215,6 +218,8 @@ private:
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;
+
+	Transform groundTruth_;
 };
 
 }

corelib/include/rtabmap/core/Transform.h

@@ -51,6 +51,8 @@ public:
 	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y,z, qx,qy,qz,qw
+	Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
 	// x,y, theta
 	Transform(float x, float y, float theta);
 
@@ -101,6 +103,7 @@ public:
 	float getNormSquared() const;
 	float getDistance(const Transform & t) const;
 	float getDistanceSquared(const Transform & t) const;
+	Transform interpolate(float t, const Transform & other) const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;
@@ -124,6 +127,14 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+
+	/**
+	 * Format (3 values): x y z
+	 * Format (6 values): x y z roll pitch yaw
+	 * Format (7 values): x y z qx qy qz qw
+	 * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+	 * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+	 */
 	static Transform fromString(const std::string & string);
 
 private: