Merge branch 'devel' of https://github.com/introlab/rtabmap into devel

CMakeLists.txt

@@ -19,7 +19,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 9)
+SET(RTABMAP_MINOR_VERSION 10)
 SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})

corelib/include/rtabmap/core/CameraModel.h

@@ -43,16 +43,29 @@ public:
 	// D is the distortion coefficients 1x5 CV_64FC1
 	// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
 	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
-	CameraModel(const std::string & name, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P);
+	CameraModel(
+			const std::string & name,
+			const cv::Size & imageSize,
+			const cv::Mat & K,
+			const cv::Mat & D,
+			const cv::Mat & R,
+			const cv::Mat & P,
+			const Transform & localTransform = Transform::getIdentity());
+
+	// minimal
+	CameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = Transform::getIdentity(),
+			double Tx = 0.0f);
 	virtual ~CameraModel() {}
 
 	bool isValid() const {return !K_.empty() &&
 									!D_.empty() &&
 									!R_.empty() &&
-									!P_.empty() &&
+									!P_.empty();}
-									imageSize_.height &&
-									imageSize_.width &&
-									!name_.empty();}
 
 	const std::string & name() const {return name_;}
 
@@ -67,6 +80,8 @@ public:
 	const cv::Mat & R() const {return R_;} //rectification matrix
 	const cv::Mat & P() const {return P_;} //projection matrix
 
+	const Transform & localTransform() const {return localTransform_;}
+
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageWeight() const {return imageSize_.height;}
@@ -74,6 +89,8 @@ public:
 	bool load(const std::string & filePath);
 	bool save(const std::string & filePath);
 
+	void scale(double scale);
+
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
@@ -87,20 +104,23 @@ private:
 	cv::Mat P_;
 	cv::Mat mapX_;
 	cv::Mat mapY_;
+	Transform localTransform_;
 };
 
 class RTABMAP_EXP StereoCameraModel
 {
 public:
 	StereoCameraModel() {}
-	StereoCameraModel(const std::string & name,
+	StereoCameraModel(
+			const std::string & name,
 			const cv::Size & imageSize1,
 			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
 			const cv::Size & imageSize2,
 			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
-			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) :
+			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
-		left_(name+"_left", imageSize1, K1, D1, R1, P1),
+			const Transform & localTransform = Transform::getIdentity()) :
-		right_(name+"_right", imageSize2, K2, D2, R2, P2),
+		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
+		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
 		name_(name),
 		R_(R),
 		T_(T),
@@ -108,9 +128,21 @@ public:
 		F_(F)
 	{
 	}
+	//minimal
+	StereoCameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(fx, fy, cx, cy, localTransform),
+		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
+	{
+	}
 	virtual ~StereoCameraModel() {}
 
-	bool isValid() const {return left_.isValid() && right_.isValid();}
+	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
 	const std::string & name() const {return name_;}
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
@@ -123,7 +155,9 @@ public:
 	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
 	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
 
-	Transform transform() const;
+	void scale(double scale);
+
+	Transform stereoTransform() const;
 
 	const CameraModel & left() const {return left_;}
 	const CameraModel & right() const {return right_;}

corelib/include/rtabmap/core/DBDriver.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
@@ -95,8 +96,7 @@ public:
 
 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const;
+	void getNodeData(int signatureId, SensorData & data) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
@@ -134,8 +134,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0;
+	virtual void getNodeDataQuery(int signatureId, SensorData & data) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

corelib/include/rtabmap/core/DBReader.h

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/OdometryEvent.h>
 
 #include <opencv2/core/core.hpp>
 
@@ -59,7 +59,7 @@ public:
 
 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	SensorData getNextData();
+	OdometryEvent getNextData();
 
 protected:
 	virtual void mainLoopBegin();

corelib/include/rtabmap/core/Link.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -42,19 +43,33 @@ public:
 		from_(0),
 		to_(0),
 		type_(kUndef),
-		rotVariance_(1.0f),
+		infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
-		transVariance_(1.0f)
 	{
 	}
-	Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
 		from_(from),
 		to_(to),
 		transform_(transform),
-		type_(type),
+		type_(type)
-		rotVariance_(rotVariance),
-		transVariance_(transVariance)
 	{
-		UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+		setInfMatrix(infMatrix);
+	}
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			double rotVariance,
+			double transVariance) :
+		from_(from),
+		to_(to),
+		transform_(transform),
+		type_(type)
+	{
+		setVariance(rotVariance, transVariance);
 	}
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -63,17 +78,44 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
-	float rotVariance() const {return rotVariance_;}
+	const cv::Mat & infMatrix() const {return infMatrix_;}
-	float transVariance() const {return transVariance_;}
+	double rotVariance() const
+	{
+		double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
+		UASSERT(min > 0.0);
+		return 1.0/min;
+	}
+	double transVariance() const
+	{
+		double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
+		UASSERT(min > 0.0);
+		return 1.0/min;
+	}
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setVariance(float rotVariance, float transVariance) {
+	void setInfMatrix(const cv::Mat & infMatrix) {
-		UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+		UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
-		rotVariance_ = rotVariance;
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		transVariance_ = transVariance;
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
+		infMatrix_ = infMatrix;
+	}
+	void setVariance(double rotVariance, double transVariance) {
+		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+		UASSERT(uIsFinite(transVariance) && transVariance>0);
+		infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
+		infMatrix_.at<double>(0,0) = 1.0/transVariance;
+		infMatrix_.at<double>(1,1) = 1.0/transVariance;
+		infMatrix_.at<double>(2,2) = 1.0/transVariance;
+		infMatrix_.at<double>(3,3) = 1.0/rotVariance;
+		infMatrix_.at<double>(4,4) = 1.0/rotVariance;
+		infMatrix_.at<double>(5,5) = 1.0/rotVariance;
 	}
 
 	Link merge(const Link & link) const
@@ -82,19 +124,19 @@ public:
 		UASSERT(type_ == link.type());
 		UASSERT(!transform_.isNull());
 		UASSERT(!link.transform().isNull());
-		UASSERT(rotVariance_ > 0 && link.rotVariance() > 0 && transVariance_ > 0 && link.transVariance() > 0);
+		UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
+		UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
 		return Link(
 				from_,
 				link.to(),
 				type_,
 				transform_ * link.transform(),
-				1.0f/(1.0f/rotVariance_   + 1.0f/link.rotVariance()),
+				infMatrix_ + link.infMatrix());
-				1.0f/(1.0f/transVariance_ + 1.0f/link.transVariance()));
 	}
 
 	Link inverse() const
 	{
-		return Link(to_, from_, type_, transform_.inverse(), rotVariance_, transVariance_);
+		return Link(to_, from_, type_, transform_.inverse(), infMatrix_);
 	}
 
 private:
@@ -102,8 +144,7 @@ private:
 	int to_;
 	Transform transform_;
 	Type type_;
-	float rotVariance_;
+	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
-	float transVariance_;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -65,7 +65,12 @@ public:
 	virtual ~Memory();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	bool update(const SensorData & data, Statistics * stats = 0);
+	bool update(const SensorData & data,
+			Statistics * stats = 0);
+	bool update(const SensorData & data,
+			const Transform & pose,
+			const cv::Mat & covariance,
+			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
@@ -81,8 +86,9 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
+	bool addLink(const Link & link);
 	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
+	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
 	void removeAllVirtualLinks();
 	void removeVirtualLinks(int signatureId);
 	std::map<int, int> getNeighborsId(
@@ -91,7 +97,7 @@ public:
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
-			bool ignoreBadSignatures = false,
+			bool ignoreIntermediateNodes = false,
 			double * dbAccessTime = 0) const;
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
@@ -131,8 +137,8 @@ public:
 			std::vector<unsigned char> & userData,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	Signature getSignatureData(int locationId, bool uncompressedData = false);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false);
-	Signature getSignatureDataConst(int locationId) const;
+	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -185,7 +191,7 @@ public:
 
 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
+	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
 	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
@@ -203,6 +209,7 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
+			const Transform & pose,
 			Statistics * stats = 0);
 
 	//keypoint stuff

corelib/include/rtabmap/core/OdometryEvent.h

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYEVENT_H_
 
 #include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/OdometryInfo.h"
 
@@ -37,20 +39,69 @@ namespace rtabmap {
 class OdometryEvent : public UEvent
 {
 public:
+	static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
+	{
+		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+		UASSERT(uIsFinite(transVariance) && transVariance>0);
+		cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+		covariance.at<double>(0,0) = transVariance;
+		covariance.at<double>(1,1) = transVariance;
+		covariance.at<double>(2,2) = transVariance;
+		covariance.at<double>(3,3) = rotVariance;
+		covariance.at<double>(4,4) = rotVariance;
+		covariance.at<double>(5,5) = rotVariance;
+		return covariance;
+	}
+public:
+	OdometryEvent() :
+		_covariance(cv::Mat::eye(6,6,CV_64FC1))
+	{
+	}
 	OdometryEvent(
-		const SensorData & data, const OdometryInfo & info = OdometryInfo()) :
+		const SensorData & data,
+		const Transform & pose,
+		const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
+		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
+			_pose(pose),
 			_info(info)
-	{}
+	{
+		UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		_covariance = covariance;
+	}
+	OdometryEvent(
+		const SensorData & data,
+		const Transform & pose,
+		double rotVariance = 1.0,
+		double transVariance = 1.0,
+		const OdometryInfo & info = OdometryInfo()) :
+			_data(data),
+			_pose(pose),
+			_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
+			_info(info)
+	{
+	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
-	bool isValid() const {return !_data.pose().isNull();}
+	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
+	const Transform & pose() const {return _pose;}
+	const cv::Mat & covariance() const {return _covariance;}
 	const OdometryInfo & info() const {return _info;}
+	double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
+	double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}
 
 private:
 	SensorData _data;
+	Transform _pose;
+	cv::Mat _covariance;
 	OdometryInfo _info;
 };
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -66,7 +66,10 @@ public:
 	virtual ~Rtabmap();
 
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
-	bool process(const SensorData & data); // for convenience
+	bool process(
+			const SensorData & data,
+			const Transform & odomPose,
+			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
@@ -117,21 +120,13 @@ public:
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
-			std::map<int, int> & mapIds,
-			std::map<int, double> & stamps,
-			std::map<int, std::string> & labels,
-			std::map<int, std::vector<unsigned char> > & userDatas,
 			bool optimized,
 			bool global) const;
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
-			std::map<int, int> & mapIds,
-			std::map<int, double> & stamps,
-			std::map<int, std::string> & labels,
-			std::map<int, std::vector<unsigned char> > & userDatas,
 			bool optimized,
-			bool global,
+			bool global,
-			bool posesConstraintsOnly = false);
+			std::map<int, Signature> * signatures = 0);
 	void clearPath();
 	bool computePath(int targetNode, bool global);
 	bool computePath(const Transform & targetPose, bool global);
@@ -166,7 +161,7 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishLastSignature;
+	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
 	float _maxTimeAllowed; // in ms

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -150,19 +150,11 @@ public:
 	RtabmapEvent3DMap(
 			const std::map<int, Signature> & signatures,
 			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
+			const std::multimap<int, Link> & constraints) :
-			const std::map<int, int> & mapIds,
-			const std::map<int, double> & stamps,
-			const std::map<int, std::string> & labels,
-			const std::map<int, std::vector<unsigned char> > & userDatas) :
 		UEvent(0),
 		_signatures(signatures),
 		_poses(poses),
-		_constraints(constraints),
+		_constraints(constraints)
-		_mapIds(mapIds),
-		_stamps(stamps),
-		_labels(labels),
-		_userDatas(userDatas)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -170,10 +162,6 @@ public:
 	const std::map<int, Signature> & getSignatures() const {return _signatures;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
 	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
-	const std::map<int, double> & getStamps() const {return _stamps;}
-	const std::map<int, std::string> & getLabels() const {return _labels;}
-	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -181,10 +169,6 @@ private:
 	std::map<int, Signature> _signatures;
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
-	std::map<int, int> _mapIds;
-	std::map<int, double> _stamps;
-	std::map<int, std::string> _labels;
-	std::map<int, std::vector<unsigned char> > _userDatas;
 };
 
 class RtabmapGlobalPathEvent : public UEvent

corelib/include/rtabmap/core/RtabmapThread.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/OdometryEvent.h"
 
 #include <stack>
 
@@ -93,8 +94,8 @@ private:
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 	void process();
-	void addData(const SensorData & data);
+	void addData(const OdometryEvent & odomEvent);
-	bool getData(SensorData & data);
+	bool getData(OdometryEvent & data);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void publishMap(bool optimized, bool full) const;
 	void publishGraph(bool optimized, bool full) const;
@@ -104,7 +105,7 @@ private:
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;
 
-	std::list<SensorData> _dataBuffer;
+	std::list<OdometryEvent> _dataBuffer;
 	UMutex _dataMutex;
 	USemaphore _dataAdded;
 	unsigned int _dataBufferMaxSize;
@@ -115,8 +116,8 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	float _rotVariance;
+	double _rotVariance;
-	float _transVariance;
+	double _transVariance;
 
 	std::vector<unsigned char> _userData;
 	UMutex _userDataMutex;

corelib/include/rtabmap/core/SensorData.h

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Transform.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
@@ -42,71 +44,133 @@ namespace rtabmap
 class RTABMAP_EXP SensorData
 {
 public:
-	SensorData(); // empty constructor
+	// empty constructor
-	SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+	SensorData();
 
-	// Metric constructor
+	// Appearance-only constructor
-	SensorData(const cv::Mat & image,
+	SensorData(
-		  const cv::Mat & depthOrRightImage,
+			const cv::Mat & image,
-		  float fx,
+			int id = 0,
-		  float fyOrBaseline,
+			double stamp = 0.0,
-		  float cx,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
-	// Metric constructor + 2d laser scan
+	// Mono constructor
-	SensorData(const cv::Mat & laserScan,
+	SensorData(
+			const cv::Mat & image,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+
+	// RGB-D constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+
+	// RGB-D constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
 			int laserScanMaxPts,
-		  const cv::Mat & image,
+			const cv::Mat & rgb,
-		  const cv::Mat & depthOrRightImage,
+			const cv::Mat & depth,
-		  float fx,
+			const CameraModel & cameraModel,
-		  float fyOrBaseline,
+			int id = 0,
-		  float cx,
+			double stamp = 0.0,
-		  float cy,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
-		  const Transform & localTransform,
+
-		  const Transform & pose,
+	// Multi-cameras RGB-D constructor
-		  float poseRotVariance,
+	SensorData(
-		  float poseTransVariance,
+			const cv::Mat & rgb,
-		  int id,
+			const cv::Mat & depth,
-		  double stamp,
+			const std::vector<CameraModel> & cameraModels,
-		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+
+	// Multi-cameras RGB-D constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
+			int laserScanMaxPts,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+
+	// Stereo constructor
+	SensorData(
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+
+	// Stereo constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
+			int laserScanMaxPts,
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	virtual ~SensorData() {}
 
-	bool isValid() const {return !_image.empty();}
+	bool isValid() const {
+		return !(_id == 0 &&
+			_stamp == 0.0 &&
+			_laserScanMaxPts == 0 &&
+			_imageRaw.empty() &&
+			_imageCompressed.empty() &&
+			_depthOrRightRaw.empty() &&
+			_depthOrRightCompressed.empty() &&
+			_laserScanRaw.empty() &&
+			_laserScanCompressed.empty() &&
+			_cameraModels.size() == 0 &&
+			!_stereoCameraModel.isValid() &&
+			_userData.size() == 0 &&
+			_keypoints.size() == 0 &&
+			_descriptors.empty());
+	}
 
-	// use isValid() instead
-	RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead.");
-
-	const cv::Mat & image() const {return _image;}
 	int id() const {return _id;}
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
-
-	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
-	void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
-	cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
-	cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
-	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
-	const cv::Mat & laserScan() const {return _laserScan;}
 	int laserScanMaxPts() const {return _laserScanMaxPts;}
-	float fx() const {return _fx;}
+
-	float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
+	const cv::Mat & imageCompressed() const {return _imageCompressed;}
-	float cx() const {return _cx;}
+	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
-	float cy() const {return _cy;}
+	const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;}
-	float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;}
+
-	float fyOrBaseline() const {return _fyOrBaseline;}
+	const cv::Mat & imageRaw() const {return _imageRaw;}
-	const Transform & pose() const {return _pose;}
+	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
-	const Transform & localTransform() const {return _localTransform;}
+	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
-	float poseRotVariance() const {return _poseRotVariance;}
+	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
-	float poseTransVariance() const {return _poseTransVariance;}
+	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
+
+	//for convenience
+	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
+	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
+
+	void uncompressData();
+	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw);
+	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const;
+
+	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
+	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
+
+	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
+	const std::vector<unsigned char> & userData() const {return _userData;}
 
 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
@@ -116,33 +180,28 @@ public:
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
-	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
-	const std::vector<unsigned char> & userData() const {return _userData;}
-
 private:
-	cv::Mat _image;
 	int _id;
 	double _stamp;
-
-	// Metric stuff
-	cv::Mat _depthOrRightImage;
-	cv::Mat _laserScan;
-	float _fx;
-	float _fyOrBaseline;
-	float _cx;
-	float _cy;
-	Transform _pose;
-	Transform _localTransform;
-	float _poseRotVariance;
-	float _poseTransVariance;
 	int _laserScanMaxPts;
 
+	cv::Mat _imageCompressed;          // compressed image
+	cv::Mat _depthOrRightCompressed;   // compressed image
+	cv::Mat _laserScanCompressed;      // compressed data
+
+	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
+	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
+	cv::Mat _laserScanRaw;      // CV_32FC2
+
+	std::vector<CameraModel> _cameraModels;
+	StereoCameraModel _stereoCameraModel;
+
+	// user data
+	std::vector<unsigned char> _userData;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;
-
-	// user data
-	std::vector<unsigned char> _userData;
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -53,23 +53,13 @@ class RTABMAP_EXP Signature
 public:
 	Signature();
 	Signature(int id,
-			int mapId,
+			int mapId = -1,
-			int weight,
+			int weight = 0,
-			double stamp,
+			double stamp = 0.0,
-			const std::string & label,
+			const std::string & label = std::string(),
-			const std::multimap<int, cv::KeyPoint> & words,
-			const std::multimap<int, pcl::PointXYZ> & words3,
 			const Transform & pose = Transform(),
 			const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
-			const cv::Mat & laserScan = cv::Mat(),
+			const SensorData & sensorData = SensorData());
-			const cv::Mat & image = cv::Mat(),
-			const cv::Mat & depth = cv::Mat(),
-			float fx = 0.0f,
-			float fy = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f,
-			const Transform & localTransform =Transform::getIdentity(),
-			int laserScanMaxPts = 0);
 	virtual ~Signature();
 
 	/**
@@ -121,41 +111,17 @@ public:
 	void setEnabled(bool enabled) {_enabled = enabled;}
 	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	void setImageCompressed(const cv::Mat & bytes) {_imageCompressed = bytes;}
-	const cv::Mat & getImageCompressed() const {return _imageCompressed;}
-	void setImageRaw(const cv::Mat & image) {_imageRaw = image;}
-	const cv::Mat & getImageRaw() const {return _imageRaw;}
 
 	//metric stuff
 	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
-	void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
-	void setLaserScanCompressed(const cv::Mat & bytes, int maxPts) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
-	void setLocalTransform(const Transform & t) {_localTransform = t;}
 	void setPose(const Transform & pose) {_pose = pose;}
-	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
-	const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
-	const cv::Mat & getLaserScanCompressed() const {return _laserScanCompressed;}
-	RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead.");
-	RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead.");
-	RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead.");
-	RTABMAP_DEPRECATED(float getDepthCy() const, "Use getCy() instead.");
-	float getFx() const {return _fx;}
-	float getFy() const {return _fy;}
-	float getCx() const {return _cx;}
-	float getCy() const {return _cy;}
-	const Transform & getPose() const {return _pose;}
-	void getPoseVariance(float & rotVariance, float & transVariance) const;
-	const Transform & getLocalTransform() const {return _localTransform;}
-	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
-	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
-	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
-	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 
-	SensorData toSensorData();
+	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
-	void uncompressData();
+	const Transform & getPose() const {return _pose;}
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw);
+	cv::Mat getPoseCovariance() const;
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const;
+
+	SensorData & sensorData() {return _sensorData;}
+	const SensorData & sensorData() const {return _sensorData;}
 
 private:
 	int _id;
@@ -173,24 +139,13 @@ private:
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
 	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
-	cv::Mat _imageCompressed; // compressed image
 
-	cv::Mat _depthCompressed; // compressed image
-	cv::Mat _laserScanCompressed; // compressed data
-	float _fx;
-	float _fy;
-	float _cx;
-	float _cy;
 	Transform _pose;
-	Transform _localTransform; // camera_link -> base_link
-	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
-	int _laserScanMaxPts;
 
-	cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
+	SensorData _sensorData;
-	cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _laserScanRaw; // CV_32FC2
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Statistics.h

@@ -136,11 +136,7 @@ public:
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
 
-	void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
+	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
-	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
-	void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
-	void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
-	void setSignature(const Signature & s) {_signature = s;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -159,11 +155,7 @@ public:
 	int loopClosureId() const {return _loopClosureId;}
 	int localLoopClosureId() const {return _localLoopClosureId;}
 
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
+	const std::map<int, Signature> & getSignatures() const {return _signatures;}
-	const std::map<int, std::string> & getLabels() const {return _labels;}
-	const std::map<int, double> & getStamps() const {return _stamps;}
-	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
-	const Signature & getSignature() const {return _signature;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -185,14 +177,7 @@ private:
 	int _loopClosureId;
 	int _localLoopClosureId;
 
-	// extended data start here...
+	std::map<int, Signature> _signatures;
-	std::map<int, int> _mapIds;
-	std::map<int, std::string> _labels;
-	std::map<int, double> _stamps;
-	std::map<int, std::vector<unsigned char> > _userDatas;
-
-	// Signature data
-	Signature _signature;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;

corelib/include/rtabmap/core/Transform.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -46,25 +47,27 @@ public:
 	Transform(float r11, float r12, float r13, float o14,
 		      float r21, float r22, float r23, float o24,
 			  float r31, float r32, float r33, float o34);
+	// should have 3 rows, 4 cols and type CV_32FC1
+	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
 
-	float r11() const {return data_[0];}
+	float r11() const {return data()[0];}
-	float r12() const {return data_[1];}
+	float r12() const {return data()[1];}
-	float r13() const {return data_[2];}
+	float r13() const {return data()[2];}
-	float r21() const {return data_[4];}
+	float r21() const {return data()[4];}
-	float r22() const {return data_[5];}
+	float r22() const {return data()[5];}
-	float r23() const {return data_[6];}
+	float r23() const {return data()[6];}
-	float r31() const {return data_[8];}
+	float r31() const {return data()[8];}
-	float r32() const {return data_[9];}
+	float r32() const {return data()[9];}
-	float r33() const {return data_[10];}
+	float r33() const {return data()[10];}
 
-	float o14() const {return data_[3];}
+	float o14() const {return data()[3];}
-	float o24() const {return data_[7];}
+	float o24() const {return data()[7];}
-	float o34() const {return data_[11];}
+	float o34() const {return data()[11];}
 
-	float & operator[](int index) {return data_[index];}
+	float & operator[](int index) {return data()[index];}
-	const float & operator[](int index) const {return data_[index];}
+	const float & operator[](int index) const {return data()[index];}
 
 	bool isNull() const;
 	bool isIdentity() const;
@@ -72,16 +75,16 @@ public:
 	void setNull();
 	void setIdentity();
 
-	const float * data() const {return data_.data();}
+	const float * data() const {return (const float *)data_.data;}
-	float * data() {return data_.data();}
+	float * data() {return (float *)data_.data;}
-	int size() const {return (int)data_.size();}
+	int size() const {return 12;}
 
-	float & x() {return data_[3];}
+	float & x() {return data()[3];}
-	float & y() {return data_[7];}
+	float & y() {return data()[7];}
-	float & z() {return data_[11];}
+	float & z() {return data()[11];}
-	const float & x() const {return data_[3];}
+	const float & x() const {return data()[3];}
-	const float & y() const {return data_[7];}
+	const float & y() const {return data()[7];}
-	const float & z() const {return data_[11];}
+	const float & z() const {return data()[11];}
 
 	float theta() const;
 
@@ -121,7 +124,7 @@ public:
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
 private:
-	std::vector<float> data_;
+	cv::Mat data_;
 };
 
 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);

corelib/include/rtabmap/core/util3d.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/SensorData.h>
 #include <opencv2/core/core.hpp>
 #include <list>
 
@@ -103,6 +104,38 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		float fx, float baseline,
 		int decimation = 1);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float voxelSize = 0.0f,
+		int samples = 0);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float voxelSize = 0.0f,
+		int samples = 0);
+
+pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth = 0,
+					const Transform & localTransform = Transform::getIdentity());
+
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform = Transform::getIdentity());
+
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,

corelib/include/rtabmap/core/util3d_conversions.h

@@ -1,57 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef UTIL3D_CONVERSIONS_H_
-#define UTIL3D_CONVERSIONS_H_
-
-#include <rtabmap/core/RtabmapExp.h>
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-#include <opencv2/core/core.hpp>
-#include <rtabmap/core/Transform.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
-cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
-
-cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform = Transform::getIdentity());
-
-} // namespace util3d
-} // namespace rtabmap
-
-#endif /* UTIL3D_CONVERSIONS_H_ */

corelib/include/rtabmap/core/util3d_features.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
 #include <list>
 
 namespace rtabmap
@@ -46,20 +47,17 @@ namespace util3d
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
+		const CameraModel & cameraModel);
-		float fy,
+
-		float cx,
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
-		float cy,
+		const std::vector<cv::KeyPoint> & keypoints,
-		const Transform & transform);
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		float fx,
+		const StereoCameraModel & stereoCameraMode);
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		const std::vector<cv::KeyPoint> & keypoints,
@@ -69,7 +67,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform = Transform::getIdentity(),
+		Transform localTransform = Transform::getIdentity(),
 		int flowWinSize = 9,
 		int flowMaxLevel = 4,
 		int flowIterations = 20,
@@ -83,7 +81,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform = Transform::getIdentity(),
+		Transform localTransform = Transform::getIdentity(),
 		int flowWinSize = 9,
 		int flowMaxLevel = 4,
 		int flowIterations = 20,
@@ -93,11 +91,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
 		const std::multimap<int, cv::KeyPoint> & kpts,
 		const std::multimap<int, cv::KeyPoint> & previousKpts,
-		float fx,
+		const CameraModel & cameraModel,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
 		Transform & cameraTransform,
 		int pnpIterations = 100,
 		float pnpReprojError = 8.0f,

corelib/src/CMakeLists.txt

@@ -35,7 +35,6 @@ SET(SRC_FILES
 	util3d_surface.cpp
 	util3d_features.cpp
 	util3d_correspondences.cpp
-	util3d_conversions.cpp
 	
 	SensorData.cpp
 	Graph.cpp

corelib/src/CameraModel.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap {
@@ -39,13 +40,21 @@ CameraModel::CameraModel() :
 
 }
 
-CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageSize, const cv::Mat & K, const cv::Mat & D, const cv::Mat & R, const cv::Mat & P) :
+CameraModel::CameraModel(
+		const std::string & cameraName,
+		const cv::Size & imageSize,
+		const cv::Mat & K,
+		const cv::Mat & D,
+		const cv::Mat & R,
+		const cv::Mat & P,
+		const Transform & localTransform) :
 		name_(cameraName),
 		imageSize_(imageSize),
 		K_(K),
 		D_(D),
 		R_(R),
-		P_(P)
+		P_(P),
+		localTransform_(localTransform)
 {
 	UASSERT(!name_.empty());
 	UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
@@ -59,6 +68,35 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS
 	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 }
 
+CameraModel::CameraModel(
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		const Transform & localTransform,
+		double Tx) :
+		K_(cv::Mat::eye(3, 3, CV_64FC1)),
+		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
+		R_(cv::Mat::eye(3, 3, CV_64FC1)),
+		P_(cv::Mat::eye(3, 4, CV_64FC1)),
+		localTransform_(localTransform)
+{
+	UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
+	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	P_.at<double>(0,0) = fx;
+	P_.at<double>(1,1) = fy;
+	P_.at<double>(0,2) = cx;
+	P_.at<double>(1,2) = cy;
+	P_.at<double>(0,3) = Tx;
+
+	K_.at<double>(0,0) = fx;
+	K_.at<double>(1,1) = fy;
+	K_.at<double>(0,2) = cx;
+	K_.at<double>(1,2) = cy;
+}
+
 bool CameraModel::load(const std::string & filePath)
 {
 	K_ = cv::Mat();
@@ -176,6 +214,22 @@ bool CameraModel::save(const std::string & filePath)
 	return false;
 }
 
+void CameraModel::scale(double scale)
+{
+	UASSERT(scale > 0.0);
+	// has only effect on K and P
+	imageSize_.width *= scale;
+	imageSize_.height *= scale;
+	K_.at<double>(0,0) *= scale;
+	K_.at<double>(1,1) *= scale;
+	K_.at<double>(0,2) *= scale;
+	K_.at<double>(1,2) *= scale;
+	P_.at<double>(0,0) *= scale;
+	P_.at<double>(1,1) *= scale;
+	P_.at<double>(0,2) *= scale;
+	P_.at<double>(1,2) *= scale;
+}
+
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 {
 	if(!mapX_.empty() && !mapY_.empty())
@@ -364,7 +418,13 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
 	return false;
 }
 
-Transform StereoCameraModel::transform() const
+void StereoCameraModel::scale(double scale)
+{
+	left_.scale(scale);
+	right_.scale(scale);
+}
+
+Transform StereoCameraModel::stereoTransform() const
 {
 	if(!R_.empty() && !T_.empty())
 	{

corelib/src/CameraThread.cpp

@@ -109,13 +109,13 @@ void CameraThread::mainLoop()
 	UDEBUG("");
 	cv::Mat rgb, depth;
 	float fx = 0.0f;
-	float fy = 0.0f;
+	float fyOrBaseline = 0.0f;
 	float cx = 0.0f;
 	float cy = 0.0f;
 	double stamp = UTimer::now();
 	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
+		_cameraRGBD->takeImage(rgb, depth, fx, fyOrBaseline, cx, cy, stamp);
 	}
 	else
 	{
@@ -125,8 +125,19 @@ void CameraThread::mainLoop()
 	if(!rgb.empty())
 	{
 		if(_cameraRGBD)
-		{
+		{
-			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, stamp);
+			SensorData data;
+			if(dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) || dynamic_cast<CameraStereoDC1394*>(_cameraRGBD))
+			{
+				//stereo
+				data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
+				UASSERT(data.stereoCameraModel().isValid());
+			}
+			else
+			{
+				data = SensorData(rgb, depth, CameraModel(fx, fyOrBaseline, cx, cy, _cameraRGBD->getLocalTransform()), ++_seq, stamp);
+				UASSERT(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid());
+			}
 			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
 		}
 		else

corelib/src/DBDriver.cpp

@@ -412,15 +412,7 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
 
 void DBDriver::getNodeData(
 		int signatureId,
-		cv::Mat & imageCompressed,
+		SensorData & data) const
-		cv::Mat & depthCompressed,
-		cv::Mat & laserScanCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		int & laserScanMaxPts) const
 {
 	bool found = false;
 	// look in the trash
@@ -428,17 +420,9 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
+		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
 		{
-			imageCompressed = s->getImageCompressed();
+			data = (SensorData)s->sensorData();
-			depthCompressed = s->getDepthCompressed();
-			laserScanCompressed = s->getLaserScanCompressed();
-			fx = s->getFx();
-			fy = s->getFy();
-			cx = s->getCx();
-			cy = s->getCy();
-			localTransform = s->getLocalTransform();
-			laserScanMaxPts = s->getLaserScanMaxPts();
 			found = true;
 		}
 	}
@@ -447,31 +431,7 @@ void DBDriver::getNodeData(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+		this->getNodeDataQuery(signatureId, data);
-		_dbSafeAccessMutex.unlock();
-	}
-}
-
-void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
-{
-	bool found = false;
-	// look in the trash
-	_trashesMutex.lock();
-	if(uContains(_trashSignatures, signatureId))
-	{
-		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
-		{
-			imageCompressed = s->getImageCompressed();
-			found = true;
-		}
-	}
-	_trashesMutex.unlock();
-
-	if(!found)
-	{
-		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed);
 		_dbSafeAccessMutex.unlock();
 	}
 }

corelib/src/DBDriverSqlite3.cpp

@@ -458,10 +458,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			if(uStrNumCmp(_version, "0.8.11") >= 0)
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				query << "SELECT image, depth, calibration, scan_max_pts, scan "
+					  << "FROM Data "
+					  << "WHERE id = ?"
+					  <<";";
+			}
+			else if(uStrNumCmp(_version, "0.8.11") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -471,7 +478,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else if(uStrNumCmp(_version, "0.7.0") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -481,7 +488,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -491,10 +498,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 		}
 		else
 		{
-			query << "SELECT data "
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
-				  << "FROM Image "
+			{
-				  << "WHERE id = ?"
+				query << "SELECT image "
-				  <<";";
+					  << "FROM Data "
+					  << "WHERE id = ?"
+					  <<";";
+			}
+			else
+			{
+				query << "SELECT data "
+					  << "FROM Image "
+					  << "WHERE id = ?"
+					  <<";";
+			}
 		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -519,13 +536,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			{
 				index = 0;
 
+				cv::Mat imageCompressed;
+				cv::Mat depthOrRightCompressed;
+				std::vector<CameraModel> models;
+				StereoCameraModel stereoModel;
+				Transform localTransform = Transform::getIdentity();
+				cv::Mat scanCompressed;
+
 				data = sqlite3_column_blob(ppStmt, index);
 				dataSize = sqlite3_column_bytes(ppStmt, index++);
 
 				//Create the image
 				if(dataSize>4 && data)
 				{
-					(*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
+					imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 				}
 
 				if(loadMetricData)
@@ -534,35 +558,92 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 
 					//Create the depth image
-					cv::Mat depthCompressed;
 					if(dataSize>4 && data)
 					{
-						depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 					}
 
-					if(uStrNumCmp(_version, "0.7.0") < 0)
+					if(uStrNumCmp(_version, "0.10.0") < 0)
 					{
-						float depthConstant = sqlite3_column_double(ppStmt, index++);
+						data = sqlite3_column_blob(ppStmt, index); // local transform
-						(*iter)->setDepthCompressed(depthCompressed, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
+						{
+							memcpy(localTransform.data(), data, dataSize);
+						}
+					}
+
+					// calibration
+					if(uStrNumCmp(_version, "0.10.0") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+						// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+						if(dataSize > 0 && data)
+						{
+							float * dataFloat = (float*)data;
+							if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+							{
+								int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+								int max = cameraCount*(4+localTransform.size());
+								for(int i=0; i<max; i+=4+localTransform.size())
+								{
+									memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+									models.push_back(CameraModel(
+											(double)dataFloat[i],
+											(double)dataFloat[i+1],
+											(double)dataFloat[i+2],
+											(double)dataFloat[i+3],
+											localTransform));
+								}
+							}
+							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+							{
+								UDEBUG("Loading calibration of a stereo camera");
+								memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+								stereoModel = StereoCameraModel(
+										dataFloat[0],  // fx
+										dataFloat[1],  // fy
+										dataFloat[2],  // cx
+										dataFloat[3],  // cy
+										dataFloat[4], // baseline
+										localTransform);
+							}
+							else
+							{
+								UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+							}
+						}
+
+					}
+					else if(uStrNumCmp(_version, "0.7.0") >= 0)
+					{
+						double fx = sqlite3_column_double(ppStmt, index++);
+						double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+						double cx = sqlite3_column_double(ppStmt, index++);
+						double cy = sqlite3_column_double(ppStmt, index++);
+						if(fyOrBaseline < 1.0)
+						{
+							//it is a baseline
+							stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+						}
+						else
+						{
+							models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+						}
 					}
 					else
 					{
-						float fx = sqlite3_column_double(ppStmt, index++);
+						float depthConstant = sqlite3_column_double(ppStmt, index++);
-						float fy = sqlite3_column_double(ppStmt, index++);
+						float fx = 1.0f/depthConstant;
-						float cx = sqlite3_column_double(ppStmt, index++);
+						float fy = 1.0f/depthConstant;
-						float cy = sqlite3_column_double(ppStmt, index++);
+						float cx = 0.0f;
-						(*iter)->setDepthCompressed(depthCompressed, fx, fy, cx, cy);
+						float cy = 0.0f;
+						models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
 					}
 
-					data = sqlite3_column_blob(ppStmt, index); // local transform
-					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					Transform localTransform;
-					if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-					{
-						memcpy(localTransform.data(), data, dataSize);
-					}
-					(*iter)->setLocalTransform(localTransform);
-
 					int laserScanMaxPts = 0;
 					if(uStrNumCmp(_version, "0.8.11") >= 0)
 					{
@@ -574,8 +655,30 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					//Create the laserScan
 					if(dataSize>4 && data)
 					{
-						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
+						scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
 					}
+
+					if(models.size())
+					{
+						(*iter)->sensorData() = SensorData(
+								scanCompressed,
+								laserScanMaxPts,
+								imageCompressed,
+								depthOrRightCompressed,
+								models,
+								(*iter)->id());
+					}
+					else
+					{
+						(*iter)->sensorData() = SensorData(
+								scanCompressed,
+								laserScanMaxPts,
+								imageCompressed,
+								depthOrRightCompressed,
+								stereoModel,
+								(*iter)->id());
+					}
+
 				}
 
 				rc = sqlite3_step(ppStmt); // next result...
@@ -596,15 +699,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 void DBDriverSqlite3::getNodeDataQuery(
 		int signatureId,
-		cv::Mat & imageCompressed,
+		 SensorData & sensorData) const
-		cv::Mat & depthCompressed,
-		cv::Mat & laserScanCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		int & laserScanMaxPts) const
 {
 	if(_ppDb)
 	{
@@ -614,10 +709,17 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.11") >= 0)
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
+		{
+			query << "SELECT image, depth, calibration, scan_max_pts, scan "
+				  << "FROM Data "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.8.11") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -627,7 +729,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else if(uStrNumCmp(_version, "0.7.0") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -637,7 +739,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -650,7 +752,15 @@ void DBDriverSqlite3::getNodeDataQuery(
 
 		const void * data = 0;
 		int dataSize = 0;
-		int index = 0;;
+		int index = 0;
+
+		cv::Mat imageCompressed;
+		cv::Mat depthOrRightCompressed;
+		std::vector<CameraModel> models;
+		StereoCameraModel stereoModel;
+		Transform localTransform = Transform::getIdentity();
+		int laserScanMaxPts;
+		cv::Mat scanCompressed;
 
 		ULOGGER_DEBUG("Loading data for %d...", signatureId);
 
@@ -675,30 +785,88 @@ void DBDriverSqlite3::getNodeDataQuery(
 			//Create the depth image
 			if(dataSize>4 && data)
 			{
-				depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 			}
 
-			if(uStrNumCmp(_version, "0.7.0") < 0)
+			if(uStrNumCmp(_version, "0.10.0") < 0)
 			{
-				float depthConstant = sqlite3_column_double(ppStmt, index++);
+				data = sqlite3_column_blob(ppStmt, index); // local transform
-				fx = 1.0f/depthConstant;
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
-				fy = 1.0f/depthConstant;
+				if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-				cx = 0.0f;
+				{
-				cy = 0.0f;
+					memcpy(localTransform.data(), data, dataSize);
+				}
+			}
+
+			// calibration
+			if(uStrNumCmp(_version, "0.10.0") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++);
+				// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+				// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+				if(dataSize > 0 && data)
+				{
+					float * dataFloat = (float*)data;
+					if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras", cameraCount);
+						int max = cameraCount*(4+localTransform.size());
+						for(int i=0; i<max; i+=4+localTransform.size())
+						{
+							memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									dataFloat[i],
+									dataFloat[i+1],
+									dataFloat[i+2],
+									dataFloat[i+3],
+									localTransform));
+						}
+					}
+					else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+					{
+						UDEBUG("Loading calibration for a stereo camera");
+						memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+						stereoModel = StereoCameraModel(
+								dataFloat[0],  // fx
+								dataFloat[1],  // fy
+								dataFloat[2],  // cx
+								dataFloat[3],  // cy
+								dataFloat[4], // baseline
+								localTransform);
+					}
+					else
+					{
+						UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+					}
+				}
+
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
+			{
+				double fx = sqlite3_column_double(ppStmt, index++);
+				double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+				double cx = sqlite3_column_double(ppStmt, index++);
+				double cy = sqlite3_column_double(ppStmt, index++);
+				if(fyOrBaseline < 1.0)
+				{
+					//it is a baseline
+					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+				}
+				else
+				{
+					models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+				}
 			}
 			else
 			{
-				fx = sqlite3_column_double(ppStmt, index++);
+				float depthConstant = sqlite3_column_double(ppStmt, index++);
-				fy = sqlite3_column_double(ppStmt, index++);
+				float fx = 1.0f/depthConstant;
-				cx = sqlite3_column_double(ppStmt, index++);
+				float fy = 1.0f/depthConstant;
-				cy = sqlite3_column_double(ppStmt, index++);
+				float cx = 0.0f;
-			}
+				float cy = 0.0f;
-
+				models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
-			data = sqlite3_column_blob(ppStmt, index); // local transform
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-			{
-				memcpy(localTransform.data(), data, dataSize);
 			}
 
 			laserScanMaxPts = 0;
@@ -712,63 +880,28 @@ void DBDriverSqlite3::getNodeDataQuery(
 			//Create the depth2d
 			if(dataSize>4 && data)
 			{
-				laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 			}
 
-			if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
+			if(models.size())
 			{
-				UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
+				sensorData = SensorData(
+						scanCompressed,
+						laserScanMaxPts,
+						imageCompressed,
+						depthOrRightCompressed,
+						models,
+						signatureId);
 			}
-
+			else
-			rc = sqlite3_step(ppStmt); // next result...
-		}
-		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-
-
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		ULOGGER_DEBUG("Time=%fs", timer.ticks());
-	}
-}
-
-void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const
-{
-	if(_ppDb)
-	{
-		UTimer timer;
-		timer.start();
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		query << "SELECT data "
-			  << "FROM Image "
-			  << "WHERE id = " << signatureId
-			  <<";";
-
-		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-
-		const void * data = 0;
-		int dataSize = 0;
-		int index = 0;;
-
-		ULOGGER_DEBUG("Loading data for %d...", signatureId);
-
-		// Process the result if one
-		rc = sqlite3_step(ppStmt);
-		if(rc == SQLITE_ROW)
-		{
-			index = 0;
-
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-
-			//Create the image
-			if(dataSize>4 && data)
 			{
-				imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				sensorData = SensorData(
+						scanCompressed,
+						laserScanMaxPts,
+						imageCompressed,
+						depthOrRightCompressed,
+						stereoModel,
+						signatureId);
 			}
 
 			rc = sqlite3_step(ppStmt); // next result...
@@ -1216,8 +1349,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						weight,
 						stamp,
 						label,
-						std::multimap<int, cv::KeyPoint>(),
-						std::multimap<int, pcl::PointXYZ>(),
 						pose,
 						userData);
 				s->setSaved(true);
@@ -1900,7 +2031,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				const std::map<int, Link> & links = (*j)->getLinks();
 				for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 				{
-					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
+					stepLink(ppStmt, i->second);
 				}
 			}
 		}
@@ -2008,7 +2139,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			const std::map<int, Link> & links = (*jter)->getLinks();
 			for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 			{
-				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
+				stepLink(ppStmt, i->second);
 			}
 		}
 		// Finalize (delete) the statement
@@ -2048,40 +2179,66 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		UDEBUG("Time=%fs", timer.ticks());
 
-		// Add images
+		if(uStrNumCmp(_version, "0.10.0") >= 0)
-		query = queryStepImage();
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		UDEBUG("Saving %d images", signatures.size());
-
-		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
-			if(!(*i)->getImageCompressed().empty())
+			// Add SensorData
+			query = queryStepSensorData();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Saving %d images", signatures.size());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				stepImage(ppStmt, (*i)->id(), (*i)->getImageCompressed());
+				if(!(*i)->sensorData().imageCompressed().empty())
+				{
+					UASSERT((*i)->id() == (*i)->sensorData().id());
+					stepSensorData(ppStmt, (*i)->sensorData());
+				}
 			}
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Time=%fs", timer.ticks());
 		}
-
+		else
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		UDEBUG("Time=%fs", timer.ticks());
-
-		// Add depths
-		query = queryStepDepth();
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
-			//metric
+			// Add images
-			if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
+			query = queryStepImage();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Saving %d images", signatures.size());
+
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
+				if(!(*i)->sensorData().imageCompressed().empty())
+				{
+					stepImage(ppStmt, (*i)->id(), (*i)->sensorData().imageCompressed());
+				}
 			}
+
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			UDEBUG("Time=%fs", timer.ticks());
+
+			// Add depths
+			query = queryStepDepth();
+			rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+			{
+				//metric
+				if(!(*i)->sensorData().depthOrRightCompressed().empty() || !(*i)->sensorData().laserScanCompressed().empty())
+				{
+					UASSERT((*i)->id() == (*i)->sensorData().id());
+					stepDepth(ppStmt, (*i)->sensorData());
+				}
+			}
+			// Finalize (delete) the statement
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		}
-		// Finalize (delete) the statement
-		rc = sqlite3_finalize(ppStmt);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 		UDEBUG("Time=%fs", timer.ticks());
 	}
@@ -2216,12 +2373,14 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 
 std::string DBDriverSqlite3::queryStepImage() const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	return "INSERT INTO Image(id, data) VALUES(?,?);";
 }
 void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 		int id,
 		const cv::Mat & imageBytes) const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
 	if(!ppStmt)
 	{
@@ -2254,6 +2413,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
 	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
 		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
@@ -2267,18 +2427,13 @@ std::string DBDriverSqlite3::queryStepDepth() const
 		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
+void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const
-		int id,
-		const cv::Mat & depthBytes,
-		const cv::Mat & depth2dBytes,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		int depth2dMaxPts) const
 {
-	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
+	UASSERT(uStrNumCmp(_version, "0.10.0") < 0);
+	UDEBUG("Save depth %d (size=%d) depth2d = %d",
+			sensorData.id(),
+			(int)sensorData.depthOrRightCompressed().cols,
+			(int)sensorData.laserScanCompressed().cols);
 	if(!ppStmt)
 	{
 		UFATAL("");
@@ -2287,12 +2442,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	int rc = SQLITE_OK;
 	int index = 1;
 
-	rc = sqlite3_bind_int(ppStmt, index++, id);
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(!depthBytes.empty())
+	if(!sensorData.depthOrRightCompressed().empty())
 	{
-		rc = sqlite3_bind_blob(ppStmt, index++, depthBytes.data, (int)depthBytes.cols, SQLITE_STATIC);
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
 	}
 	else
 	{
@@ -2300,11 +2455,33 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	float fx=0, fyOrBaseline=0, cx=0, cy=0;
+	Transform localTransform = Transform::getIdentity();
+	if(sensorData.cameraModels().size())
+	{
+		UASSERT_MSG(sensorData.cameraModels().size() == 1,
+				uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
+
+		fx = sensorData.cameraModels()[0].fx();
+		fyOrBaseline = sensorData.cameraModels()[0].fy();
+		cx = sensorData.cameraModels()[0].cx();
+		cy = sensorData.cameraModels()[0].cy();
+		localTransform = sensorData.cameraModels()[0].localTransform();
+	}
+	else if(sensorData.stereoCameraModel().isValid())
+	{
+		fx = sensorData.stereoCameraModel().left().fx();
+		fyOrBaseline = sensorData.stereoCameraModel().baseline();
+		cx = sensorData.stereoCameraModel().left().cx();
+		cy = sensorData.stereoCameraModel().left().cy();
+		localTransform = sensorData.stereoCameraModel().left().localTransform();
+	}
+
 	if(uStrNumCmp(_version, "0.7.0") >= 0)
 	{
 		rc = sqlite3_bind_double(ppStmt, index++, fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		rc = sqlite3_bind_double(ppStmt, index++, fy);
+		rc = sqlite3_bind_double(ppStmt, index++, fyOrBaseline);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		rc = sqlite3_bind_double(ppStmt, index++, cx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2320,9 +2497,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(!depth2dBytes.empty())
+	if(!sensorData.laserScanCompressed().empty())
 	{
-		rc = sqlite3_bind_blob(ppStmt, index++, depth2dBytes.data, (int)depth2dBytes.cols, SQLITE_STATIC);
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
 	}
 	else
 	{
@@ -2332,7 +2509,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
-		rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
+		rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 
@@ -2344,6 +2521,116 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepSensorData() const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
+}
+void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
+		const SensorData & sensorData) const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	UDEBUG("Save sensor data %d (image=%d depth=%d) depth2d = %d",
+			sensorData.id(),
+			(int)sensorData.imageCompressed().cols,
+			(int)sensorData.depthOrRightCompressed().cols,
+			(int)sensorData.laserScanCompressed().cols);
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	// id
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.id());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// image
+	if(!sensorData.imageCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.imageCompressed().data, (int)sensorData.imageCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// depth or right image
+	if(!sensorData.depthOrRightCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthOrRightCompressed().data, (int)sensorData.depthOrRightCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// calibration
+	std::vector<float> calibration;
+	// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+	if(sensorData.cameraModels().size())
+	{
+		calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+		{
+			const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
+			calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
+			calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
+			calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+			calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+			memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
+		}
+	}
+	else if(sensorData.stereoCameraModel().isValid())
+	{
+		const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
+		calibration.resize(5+localTransform.size());
+		calibration[0] = sensorData.stereoCameraModel().left().fx();
+		calibration[1] = sensorData.stereoCameraModel().left().fy();
+		calibration[2] = sensorData.stereoCameraModel().left().cx();
+		calibration[3] = sensorData.stereoCameraModel().left().cy();
+		calibration[4] = sensorData.stereoCameraModel().baseline();
+		memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
+	}
+
+	if(calibration.size())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, calibration.data(), calibration.size()*sizeof(float), SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// scan_max_pts
+	rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	// scan
+	if(!sensorData.laserScanCompressed().empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, sensorData.laserScanCompressed().data, (int)sensorData.laserScanCompressed().cols, SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+	}
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	//step
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+}
+
 std::string DBDriverSqlite3::queryStepLink() const
 {
 	if(uStrNumCmp(_version, "0.8.4") >= 0)
@@ -2361,21 +2648,16 @@ std::string DBDriverSqlite3::queryStepLink() const
 }
 void DBDriverSqlite3::stepLink(
 		sqlite3_stmt * ppStmt,
-		int fromId,
+		const Link & link) const
-		int toId,
-		Link::Type type,
-		float rotVariance,
-		float transVariance,
-		const Transform & transform) const
 {
 	if(!ppStmt)
 	{
 		UFATAL("");
 	}
-	UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
+	UDEBUG("Save link from %d to %d, type=%d", link.from(), link.to(), link.type());
 
 	// Don't save virtual links
-	if(type==Link::kVirtualClosure)
+	if(link.type()==Link::kVirtualClosure)
 	{
 		UDEBUG("Virtual link ignored....");
 		return;
@@ -2383,27 +2665,27 @@ void DBDriverSqlite3::stepLink(
 
 	int rc = SQLITE_OK;
 	int index = 1;
-	rc = sqlite3_bind_int(ppStmt, index++, fromId);
+	rc = sqlite3_bind_int(ppStmt, index++, link.from());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	rc = sqlite3_bind_int(ppStmt, index++, toId);
+	rc = sqlite3_bind_int(ppStmt, index++, link.to());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	rc = sqlite3_bind_int(ppStmt, index++, type);
+	rc = sqlite3_bind_int(ppStmt, index++, link.type());
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 	if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-		rc = sqlite3_bind_double(ppStmt, index++, transVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.transVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 	else if(uStrNumCmp(_version, "0.7.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
+		rc = sqlite3_bind_double(ppStmt, index++, link.rotVariance()<link.transVariance()?link.rotVariance():link.transVariance());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 
-	rc = sqlite3_bind_blob(ppStmt, index++, transform.data(), transform.size()*sizeof(float), SQLITE_STATIC);
+	rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 	rc=sqlite3_step(ppStmt);

corelib/src/DBDriverSqlite3.h

@@ -71,18 +71,7 @@ private:
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
-	virtual void getNodeDataQuery(
+	virtual void getNodeDataQuery(int signatureId, SensorData & data) const;
-			int signatureId,
-			cv::Mat & imageCompressed,
-			cv::Mat & depthCompressed,
-			cv::Mat & laserScanCompressed,
-			float & fx,
-			float & fy,
-			float & cx,
-			float & cy,
-			Transform & localTransform,
-			int & laserScanMaxPts) const;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -94,6 +83,7 @@ private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepSensorData() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
@@ -102,18 +92,9 @@ private:
 			sqlite3_stmt * ppStmt,
 			int id,
 			const cv::Mat & imageBytes) const;
-	void stepDepth(
+	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
-			sqlite3_stmt * ppStmt,
+	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
-			int id,
+	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
-			const cv::Mat & depthBytes,
-			const cv::Mat & depth2dBytes,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			const Transform & localTransform,
-			int depth2dMaxPts) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 

corelib/src/DBReader.cpp

@@ -148,39 +148,39 @@ void DBReader::mainLoopBegin()
 
 void DBReader::mainLoop()
 {
-	SensorData data = this->getNextData();
+	OdometryEvent odom = this->getNextData();
-	if(data.isValid())
+	if(odom.data().id())
 	{
 		int goalId = 0;
-		double previousStamp = data.stamp();
+		double previousStamp = odom.data().stamp();
-		data.setStamp(UTimer::now());
+		odom.data().setStamp(UTimer::now());
-		if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
+		if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0)
 		{
 			//GOAL format detected, remove it from the user data and send it as goal event
-			std::string goalStr = uBytes2Str(data.userData());
+			std::string goalStr = uBytes2Str(odom.data().userData());
 			if(!goalStr.empty())
 			{
 				std::list<std::string> strs = uSplit(goalStr, ':');
 				if(strs.size() == 2)
 				{
 					goalId = atoi(strs.rbegin()->c_str());
-					data.setUserData(std::vector<unsigned char>());
+					odom.data().setUserData(std::vector<unsigned char>());
 				}
 			}
 		}
 		if(!_odometryIgnored)
 		{
-			if(data.pose().isNull())
+			if(odom.pose().isNull())
 			{
 				UWARN("Reading the database: odometry is null! "
 					  "Please set \"Ignore odometry = true\" if there is "
 					  "no odometry in the database.");
 			}
-			this->post(new OdometryEvent(data));
+			this->post(new OdometryEvent(odom));
 		}
 		else
 		{
-			this->post(new CameraEvent(data));
+			this->post(new CameraEvent(odom.data()));
 		}
 
 		if(goalId > 0)
@@ -242,31 +242,26 @@ void DBReader::mainLoop()
 
 }
 
-SensorData DBReader::getNextData()
+OdometryEvent DBReader::getNextData()
 {
-	SensorData data;
+	OdometryEvent odom;
 	if(_dbDriver)
 	{
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
-			cv::Mat imageBytes;
-			cv::Mat depthBytes;
-			cv::Mat laserScanBytes;
 			int mapId;
-			float fx,fy,cx,cy;
-			Transform localTransform, pose;
-			float rotVariance = 1.0f;
-			float transVariance = 1.0f;
 			std::vector<unsigned char> userData;
-			int laserScanMaxPts = 0;
+			SensorData data;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+			_dbDriver->getNodeData(*_currentId, data);
 
 			// info
+			Transform pose;
 			int weight;
 			std::string label;
 			double stamp;
 			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
 
+			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
 			if(!_odometryIgnored)
 			{
 				std::map<int, Link> links;
@@ -274,8 +269,7 @@ SensorData DBReader::getNextData()
 				if(links.size())
 				{
 					// assume the first is the backward neighbor, take its variance
-					rotVariance = links.begin()->second.rotVariance();
+					infMatrix = links.begin()->second.infMatrix();
-					transVariance = links.begin()->second.transVariance();
 				}
 			}
 			else
@@ -285,7 +279,7 @@ SensorData DBReader::getNextData()
 
 			int seq = *_currentId;
 			++_currentId;
-			if(imageBytes.empty())
+			if(data.imageCompressed().empty())
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
@@ -339,33 +333,16 @@ SensorData DBReader::getNextData()
 
 			if(!this->isKilled())
 			{
-				rtabmap::CompressionThread ctImage(imageBytes, true);
+				data.uncompressData();
-				rtabmap::CompressionThread ctDepth(depthBytes, true);
+				data.setId(seq);
-				rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
+				data.setStamp(stamp);
-				ctImage.start();
+				data.setUserData(userData);
-				ctDepth.start();
+				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d",
-				ctLaserScan.start();
+						data.laserScanRaw().empty()?0:1,
-				ctImage.join();
+						data.imageRaw().empty()?0:1,
-				ctDepth.join();
+						data.depthOrRightRaw().empty()?0:1);
-				ctLaserScan.join();
+
-				data = SensorData(
+				odom = OdometryEvent(data, pose, infMatrix.inv());
-						ctLaserScan.getUncompressedData(),
-						laserScanMaxPts,
-						ctImage.getUncompressedData(),
-						ctDepth.getUncompressedData(),
-						fx,fy,cx,cy,
-						localTransform,
-						pose,
-						rotVariance,
-						transVariance,
-						seq,
-						stamp,
-						userData);
-				UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
-						data.laserScan().empty()?0:1,
-						data.image().empty()?0:1,
-						data.depth().empty()?0:1,
-						data.rightImage().empty()?0:1);
 			}
 		}
 	}
@@ -373,7 +350,7 @@ SensorData DBReader::getNextData()
 	{
 		UERROR("Not initialized...");
 	}
-	return data;
+	return odom;
 }
 
 } /* namespace rtabmap */

corelib/src/Graph.cpp

@@ -263,20 +263,23 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
 				AISNavigation::TreePoseGraph2::InformationMatrix inf;
 				//Identity:
-				inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
+				if(isCovarianceIgnored())
-				inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
-				inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
-				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
+					inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
-					{
+					inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
-						inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
+					inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
-						inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
+				}
-					}
+				else
-					if(iter->second.rotVariance()>0)
+				{
-					{
+					inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
-						inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
+					inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
-					}
+					inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
+					inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
+					inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
+					inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
+					inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
+					inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
+					inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				int id1 = iter->first;
@@ -304,18 +307,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.rotVariance()>0)
+					memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-					{
-						inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
-						inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
-						inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
-					}
-					if(iter->second.transVariance()>0)
-					{
-						inf[3][3] = 1.0f/iter->second.transVariance(); // x
-						inf[4][4] = 1.0f/iter->second.transVariance(); // y
-						inf[5][5] = 1.0f/iter->second.transVariance(); // z
-					}
 				}
 
 				int id1 = iter->first;
@@ -491,7 +483,7 @@ bool TOROOptimizer::saveGraph(
 		{
 			float x,y,z, yaw,pitch,roll;
 			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
-			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
+			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					iter->first,
 					iter->second.to(),
 					x,
@@ -500,12 +492,27 @@ bool TOROOptimizer::saveGraph(
 					roll,
 					pitch,
 					yaw,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
+					iter->second.infMatrix().at<double>(0,0),
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
+					iter->second.infMatrix().at<double>(0,1),
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
+					iter->second.infMatrix().at<double>(0,2),
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
+					iter->second.infMatrix().at<double>(0,3),
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
+					iter->second.infMatrix().at<double>(0,4),
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f);
+					iter->second.infMatrix().at<double>(0,5),
+					iter->second.infMatrix().at<double>(1,1),
+					iter->second.infMatrix().at<double>(1,2),
+					iter->second.infMatrix().at<double>(1,3),
+					iter->second.infMatrix().at<double>(1,4),
+					iter->second.infMatrix().at<double>(1,5),
+					iter->second.infMatrix().at<double>(2,2),
+					iter->second.infMatrix().at<double>(2,3),
+					iter->second.infMatrix().at<double>(2,4),
+					iter->second.infMatrix().at<double>(2,5),
+					iter->second.infMatrix().at<double>(3,3),
+					iter->second.infMatrix().at<double>(3,4),
+					iter->second.infMatrix().at<double>(3,5),
+					iter->second.infMatrix().at<double>(4,4),
+					iter->second.infMatrix().at<double>(4,5),
+					iter->second.infMatrix().at<double>(5,5));
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
 		fclose(file);
@@ -689,15 +696,15 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
+					information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
-					{
+					information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
+					information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
+					information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
-					}
+					information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
-					if(iter->second.rotVariance()>0)
+					information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
-					{
+					information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
-						information(2,2) = 1.0f/iter->second.rotVariance(); // theta
+					information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
-					}
+					information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				g2o::EdgeSE2 * e = new g2o::EdgeSE2();
@@ -716,18 +723,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-					{
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
-						information(2,2) = 1.0f/iter->second.transVariance(); // z
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						information(3,3) = 1.0f/iter->second.rotVariance(); // roll
-						information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
-						information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
-					}
 				}
 
 				Eigen::Affine3d a = iter->second.transform().toEigen3d();

corelib/src/Odometry.cpp

@@ -161,16 +161,16 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		_pose.setIdentity(); // initialized
 	}
 
-	UASSERT(!data.image().empty());
+	UASSERT(!data.imageRaw().empty());
 	if(dynamic_cast<OdometryMono*>(this) == 0)
 	{
-		UASSERT(!data.depthOrRightImage().empty());
+		UASSERT(!data.depthOrRightRaw().empty());
 	}
 
-	if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
+	if(!data.stereoCameraModel().isValid() &&
+	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
 	{
-		UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
+		UERROR("Rectified images required! Calibrate your camera.");
-				data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
 		return Transform();
 	}
 

corelib/src/OdometryBOW.cpp

@@ -160,8 +160,15 @@ Transform OdometryBOW::computeTransform(
 			{
 				if(this->isPnPEstimationUsed())
 				{
-					if((int)newSignature->getWords().size() >= this->getMinInliers())
+					if(data.cameraModels().size() > 1)
 					{
+						UERROR("PnP cannot be used on multi-cameras setup.");
+					}
+					else if((int)newSignature->getWords().size() >= this->getMinInliers())
+					{
+						UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
+						const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
+
 						// find correspondences
 						std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
 						std::vector<cv::Point3f> objectPoints(ids.size());
@@ -194,11 +201,8 @@ Transform OdometryBOW::computeTransform(
 						if((int)matches.size() >= this->getMinInliers())
 						{
 							//PnPRansac
-							cv::Mat K = (cv::Mat_<double>(3,3) <<
+							cv::Mat K = cameraModel.K();
-								data.fx(), 0, data.cx(),
+							Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
-								0, data.fy()>0?data.fy():data.fx(), data.cy(),
-								0, 0, 1);
-							Transform guess = (this->getPose() * data.localTransform()).inverse();
 							cv::Mat R = (cv::Mat_<double>(3,3) <<
 									(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 									(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -229,7 +233,7 @@ Transform OdometryBOW::computeTransform(
 											   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
 								// make it incremental
-								transform = (data.localTransform() * pnp * this->getPose()).inverse();
+								transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse();
 
 								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
 

corelib/src/OdometryICP.cpp

@@ -72,25 +72,32 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	bool hasConverged = false;
 	double variance = 0;
 	unsigned int minPoints = 100;
-	if(!data.depth().empty())
+	if(!data.depthOrRightRaw().empty())
 	{
-		if(data.depth().type() == CV_8UC1)
+		if(data.depthOrRightRaw().type() == CV_8UC1)
 		{
 			UERROR("ICP 3D cannot be done on stereo images!");
 			return output;
 		}
 
+		if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
+		{
+			UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
+			return output;
+		}
+		const CameraModel & cameraModel = data.cameraModels()[0];
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-						data.depth(),
+						data.depthOrRightRaw(),
-						data.fx(),
+						cameraModel.fx(),
-						data.fy(),
+						cameraModel.fy(),
-						data.cx(),
+						cameraModel.cx(),
-						data.cy(),
+						cameraModel.cy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
 						_samples,
-						data.localTransform());
+						cameraModel.localTransform());
 
 		if(_pointToPlane)
 		{

corelib/src/OdometryMono.cpp

@@ -147,11 +147,24 @@ void OdometryMono::reset(const Transform & initialPose)
 
 Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
 {
-	UASSERT(!data.image().empty());
+	Transform output;
-	UASSERT(data.fx());
+
+	if(data.imageRaw().empty())
+	{
+		UERROR("Image empty! Cannot compute odometry...");
+		return output;
+	}
+
+	if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid())))
+	{
+		UERROR("Odometry cannot be done without calibration or on multi-camera!");
+		return output;
+	}
+
+
+	const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
 
 	UTimer timer;
-	Transform output;
 
 	int inliers = 0;
 	int correspondences = 0;
@@ -159,13 +172,13 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 	cv::Mat newFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newFrame = data.image().clone();
+		newFrame = data.imageRaw().clone();
 	}
 
 	if(memory_->getStMem().size() >= 1)
@@ -190,11 +203,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				nFeatures = (int)newS->getWords().size();
 				if((int)newS->getWords().size() > this->getMinInliers())
 				{
-					cv::Mat K = (cv::Mat_<double>(3,3) <<
+					cv::Mat K = cameraModel.K();
-						data.fx(), 0, data.cx(),
+					Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
-						0, data.fy()==0?data.fx():data.fy(), data.cy(),
-						0, 0, 1);
-					Transform guess = (this->getPose() * data.localTransform()).inverse();
 					cv::Mat R = (cv::Mat_<double>(3,3) <<
 							(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 							(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -216,7 +226,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					UDEBUG("project points to previous image");
 					std::vector<cv::Point2f> prevImagePoints;
 					const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
-					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
+					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
 					cv::Mat prevR = (cv::Mat_<double>(3,3) <<
 							(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
 							(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
@@ -240,8 +250,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					{
 						if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
 						   uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
-						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
+						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
-						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
+						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
 						{
 							refCorners[oi] = prevImagePoints[i];
 							newCorners[oi] = imagePoints[i];
@@ -273,7 +283,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					std::vector<float> err;
 					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 					cv::calcOpticalFlowPyrLK(
-							prevS->getImageRaw(),
+							prevS->sensorData().imageRaw(),
 							newFrame,
 							refCorners,
 							newCorners,
@@ -357,7 +367,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
 													  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 													  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
-							output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
+							output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
 
 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
@@ -402,9 +412,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
 										previousS->getWords(),
 										newS->getWords(),
-										data.fx(), data.fy()?data.fy():data.fx(),
+										cameraModel,
-										data.cx(), data.cy(),
-										data.localTransform(),
 										cameraTransform,
 										this->getIterations(),
 										this->getPnPReprojError(),
@@ -515,7 +523,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 			std::vector<float> err;
 			UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 			cv::calcOpticalFlowPyrLK(
-					refS->getImageRaw(),
+					refS->sensorData().imageRaw(),
 					newFrame,
 					refCorners,
 					refCornersGuess,
@@ -652,10 +660,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 						//UDEBUG("Correcting matches...done!");
 
 						UDEBUG("Computing P...");
-						cv::Mat K = (cv::Mat_<double>(3,3) <<
+						cv::Mat K = cameraModel.K();
-									data.fx(), 0, data.cx(),
-									0, data.fy()==0?data.fx():data.fy(), data.cy(),
-									0, 0, 1);
 
 						cv::Mat Kinv = K.inv();
 						cv::Mat E = K.t()*F*K;
@@ -716,7 +721,15 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									(*inliersRef)[oi] = cloud->at(i);
 									if(!refDepth_.empty())
 									{
-										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
+										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(
+												refDepth_,
+												refCorners[i].x,
+												refCorners[i].y,
+												cameraModel.cx(),
+												cameraModel.cy(),
+												cameraModel.fx(),
+												cameraModel.fy(),
+												true);
 									}
 									++oi;
 								}
@@ -824,7 +837,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 											  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 											  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-								output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
+								output = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse();
 								if(output.getNorm() < minTranslation_*5)
 								{
 									reject = true;
@@ -844,7 +857,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										int index  =inliersPnP.at(i);
 										int id = cornerIds[index];
 										UASSERT(id > 0 && id <= *wordsId.rbegin());
-										pcl::PointXYZ pt =  util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
+										pcl::PointXYZ pt =  util3d::transformPoint(
+												pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z),
+												this->getPose()*cameraModel.localTransform());
 										localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
 										keyFrameWords3D.insert(std::make_pair(id, pt));
 									}
@@ -890,7 +905,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				{
 					cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
 				}
-				refDepth_ = data.depth().clone();
+				refDepth_ = data.depthOrRightRaw().clone();
 				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
 			}
 			else

corelib/src/OdometryOpticalFlow.cpp

@@ -124,6 +124,17 @@ Transform OdometryOpticalFlow::computeTransform(
 {
 	UTimer timer;
 	Transform output;
+	if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
+	{
+		UERROR("Calibrated stereo camera required");
+		return output;
+	}
+	if(!data.depthRaw().empty() &&
+		(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
+	{
+		UERROR("Calibrated camera required (multi-cameras not supported).");
+		return output;
+	}
 
 	double variance = 0;
 	int inliers = 0;
@@ -136,20 +147,20 @@ Transform OdometryOpticalFlow::computeTransform(
 
 	cv::Mat newLeftFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newLeftFrame = data.image().clone();
+		newLeftFrame = data.imageRaw().clone();
 	}
 
 	std::vector<cv::Point2f> newCorners;
 	UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
-			(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightImage().empty()?0:1);
+			(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1);
 	if(!refFrame_.empty() &&
-	   !data.depthOrRightImage().empty() &&
+	   ((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) &&
 	   refCorners_.size() &&
 	   refCorners3D_->size())
 	{
@@ -158,11 +169,9 @@ Transform OdometryOpticalFlow::computeTransform(
 
 		// make guess
 		bool flowGuessByMotion = true;
-		cv::Mat K = (cv::Mat_<double>(3,3) <<
+		cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
-			data.fx(), 0, data.cx(),
+		Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
-			0, data.fx(), data.cy(),
+		Transform guess = (this->previousTransform() * localTransform).inverse();
-			0, 0, 1);
-		Transform guess = (this->previousTransform() * data.localTransform()).inverse();
 		cv::Mat R = (cv::Mat_<double>(3,3) <<
 				(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 				(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -263,7 +272,7 @@ Transform OdometryOpticalFlow::computeTransform(
 					if((int)inliersV.size() >= this->getMinInliers())
 					{
 						// make it incremental
-						output = (data.localTransform() * pnp).inverse();
+						output = (localTransform * pnp).inverse();
 						variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
 					}
 					else
@@ -294,17 +303,17 @@ Transform OdometryOpticalFlow::computeTransform(
 					info->newCorners.resize(newCornersKept.size());
 				}
 				int oi = 0;
-				if(!data.rightImage().empty())
+				if(!data.rightRaw().empty())
 				{
 					// stereo
 					pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
 							newCornersKept,
 							newLeftFrame,
-							data.rightImage(),
+							data.rightRaw(),
-							data.fx(),
+							data.stereoCameraModel().left().fx(),
-							data.baseline(),
+							data.stereoCameraModel().baseline(),
-							data.cx(),
+							data.stereoCameraModel().left().cx(),
-							data.cy(),
+							data.stereoCameraModel().left().cy(),
 							Transform::getIdentity(),
 							stereoWinSize_,
 							stereoMaxLevel_,
@@ -319,7 +328,7 @@ Transform OdometryOpticalFlow::computeTransform(
 						{
 							//Add 3D correspondences!
 							correspondencesRef->at(oi) = refCorners3DKept->at(i);
-							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), data.localTransform());
+							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), localTransform);
 							if(this->isInfoDataFilled() && info)
 							{
 								info->refCorners[oi] = refCornersKept[i];
@@ -334,17 +343,18 @@ Transform OdometryOpticalFlow::computeTransform(
 					//depth
 					for(unsigned int i=0; i<newCornersKept.size(); ++i)
 					{
-						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depth().cols)) &&
+						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depthRaw().cols)) &&
-						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depth().rows)))
+						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depthRaw().rows)))
 						{
-							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCornersKept[i].x, newCorners[i].y,
+							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depthRaw(), newCornersKept[i].x, newCorners[i].y,
-									data.cx(), data.cy(), data.fx(), data.fy(), true);
+									data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), true);
 							if(pcl::isFinite(pt) &&
 								(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
 							{
 								//Add 3D correspondences!
 								correspondencesRef->at(oi) = refCorners3DKept->at(i);
-								correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform());
+								correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform);
+
 								if(this->isInfoDataFilled() && info)
 								{
 									info->refCorners[oi] = refCornersKept[i];
@@ -444,17 +454,17 @@ Transform OdometryOpticalFlow::computeTransform(
 			newCorners3D->resize(newCorners.size());
 			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
 			int oi=0;
-			if(!data.rightImage().empty())
+			if(!data.rightRaw().empty())
 			{
 				/// stereo
 				pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
 						newCorners,
 						newLeftFrame,
-						data.rightImage(),
+						data.rightRaw(),
-						data.fx(),
+						data.stereoCameraModel().left().fx(),
-						data.baseline(),
+						data.stereoCameraModel().baseline(),
-						data.cx(),
+						data.stereoCameraModel().left().cx(),
-						data.cy(),
+						data.stereoCameraModel().left().cy(),
 						Transform::getIdentity(),
 						stereoWinSize_,
 						stereoMaxLevel_,
@@ -467,7 +477,7 @@ Transform OdometryOpticalFlow::computeTransform(
 					if(pcl::isFinite(refCorners3DTmp->at(i)) &&
 						(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
 					{
-						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.localTransform());
+						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform());
 						newCornersFiltered[oi] = newCorners[i];
 						++oi;
 					}
@@ -478,15 +488,22 @@ Transform OdometryOpticalFlow::computeTransform(
 				// depth
 				for(unsigned int i=0; i<newCorners.size(); ++i)
 				{
-					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
+					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
-					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
+					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
 					{
-						pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
+						pcl::PointXYZ pt = util3d::projectDepthTo3D(
-								data.cx(), data.cy(), data.fx(), data.fy(), true);
+								data.depthRaw(),
+								newCorners[i].x,
+								newCorners[i].y,
+								data.cameraModels()[0].cx(),
+								data.cameraModels()[0].cy(),
+								data.cameraModels()[0].fx(),
+								data.cameraModels()[0].fy(),
+								true);
 						if(pcl::isFinite(pt) &&
 							(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
 						{
-							newCorners3D->at(oi) = util3d::transformPoint(pt, data.localTransform());
+							newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
 							newCornersFiltered[oi] = newCorners[i];
 							++oi;
 						}

corelib/src/OdometryThread.cpp

@@ -97,8 +97,9 @@ void OdometryThread::mainLoop()
 	{
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
-		data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
+		// a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(data, info));
+		double variance = info.variance>0?info.variance:1;
+		this->post(new OdometryEvent(data, pose, variance, variance, info));
 	}
 }
 
@@ -106,7 +107,7 @@ void OdometryThread::addData(const SensorData & data)
 {
 	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 	{
-		if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
 			return;
@@ -114,7 +115,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
-		if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 			return;

corelib/src/Rtabmap.cpp

@@ -73,7 +73,7 @@ namespace rtabmap
 
 Rtabmap::Rtabmap() :
 	_publishStats(Parameters::defaultRtabmapPublishStats()),
-	_publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
+	_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
 	_publishPdf(Parameters::defaultRtabmapPublishPdf()),
 	_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
@@ -372,7 +372,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 
 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
-	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
+	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
 	Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
@@ -792,7 +792,10 @@ void Rtabmap::resetMemory()
 //============================================================
 // MAIN LOOP
 //============================================================
-bool Rtabmap::process(const SensorData & data)
+bool Rtabmap::process(
+		const SensorData & data,
+		const Transform & odomPose,
+		const cv::Mat & covariance)
 {
 	UDEBUG("");
 
@@ -863,7 +866,7 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	if(_rgbdSlamMode)
 	{
-		if(data.pose().isNull())
+		if(odomPose.isNull())
 		{
 			UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
 				   "Image %d is ignored!", data.id());
@@ -877,7 +880,7 @@ bool Rtabmap::process(const SensorData & data)
 				const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
 
 				// look for identity
-				if(!lastPose.isIdentity() && data.pose().isIdentity())
+				if(!lastPose.isIdentity() && odomPose.isIdentity())
 				{
 					int mapId = triggerNewMap();
 					UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
@@ -885,7 +888,7 @@ bool Rtabmap::process(const SensorData & data)
 				else if(_newMapOdomChangeDistance > 0.0)
 				{
 					// look for large change
-					Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
+					Transform lastPoseToNewPose = lastPose.inverse() * odomPose;
 					float x,y,z, roll,pitch,yaw;
 					lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 					if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
@@ -895,7 +898,7 @@ bool Rtabmap::process(const SensorData & data)
 								_newMapOdomChangeDistance,
 								mapId,
 								lastPose.prettyPrint().c_str(),
-								data.pose().prettyPrint().c_str());
+								odomPose.prettyPrint().c_str());
 					}
 				}
 			}
@@ -908,16 +911,14 @@ bool Rtabmap::process(const SensorData & data)
 	ULOGGER_INFO("Updating memory...");
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->update(data, &statistics_))
+		if(!_memory->update(data, odomPose, covariance, &statistics_))
 		{
 			return false;
 		}
 	}
 	else
 	{
-		SensorData dataWithoutOdom = data;
+		if(!_memory->update(data, Transform(), cv::Mat(), &statistics_))
-		dataWithoutOdom.setPose(Transform(), 1, 1);
-		if(!_memory->update(dataWithoutOdom, &statistics_))
 		{
 			return false;
 		}
@@ -929,6 +930,7 @@ bool Rtabmap::process(const SensorData & data)
 	{
 		UFATAL("Not supposed to be here...last signature is null?!?");
 	}
+
 	ULOGGER_INFO("Processing signature %d", signature->id());
 	timeMemoryUpdate = timer.ticks();
 	ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
@@ -980,7 +982,7 @@ bool Rtabmap::process(const SensorData & data)
 		//============================================================
 		if(_poseScanMatching &&
 			signature->getLinks().size() == 1 &&
-			!signature->getLaserScanCompressed().empty() &&
+			!signature->sensorData().laserScanCompressed().empty() &&
 			rehearsedId == 0) // don't do it if rehearsal happened
 		{
 			UINFO("Odometry correction by scan matching");
@@ -1023,13 +1025,13 @@ bool Rtabmap::process(const SensorData & data)
 
 			Link tmp = signature->getLinks().begin()->second.inverse();
 
-			// if the previous signature is a bad signature, remove it from the local graph
+			// if the previous node is an intermediate node, remove it from the local graph
 			if(_constraints.size() &&
 			   _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
 			{
 				const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
 				UASSERT(s!=0);
-				if(s->isBadSignature())
+				if(s->getWeight() == -1)
 				{
 					tmp = _constraints.rbegin()->second.merge(tmp);
 					_optimizedPoses.erase(s->id());
@@ -1070,7 +1072,7 @@ bool Rtabmap::process(const SensorData & data)
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1470,17 +1472,21 @@ bool Rtabmap::process(const SensorData & data)
 					{
 						if(immunizedLocally >= maxLocalLocationsImmunized)
 						{
-							UWARN("Could not immunize the whole local path (%d) between "
+							// set 20 to avoid this warning when starting mapping
-								  "%d and %d (max location immunized=%d). You may want "
+							if(maxLocalLocationsImmunized > 20)
-								  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
+							{
-								  "to be able to immunize longer paths.",
+								UWARN("Could not immunize the whole local path (%d) between "
-									(int)path.size(),
+									  "%d and %d (max location immunized=%d). You may want "
-									nearestId,
+									  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
-									signature->id(),
+									  "to be able to immunize longer paths.",
-									maxLocalLocationsImmunized,
+										(int)path.size(),
-									_localImmunizationRatio,
+										nearestId,
-									maxLocalLocationsImmunized,
+										signature->id(),
-									(int)_memory->getWorkingMem().size());
+										maxLocalLocationsImmunized,
+										_localImmunizationRatio,
+										maxLocalLocationsImmunized,
+										(int)_memory->getWorkingMem().size());
+							}
 							break;
 						}
 						else if(!_memory->isInSTM(iter->first))
@@ -1638,16 +1644,13 @@ bool Rtabmap::process(const SensorData & data)
 				// Add signatures
 				SensorData dataFrom = data;
 				dataFrom.setId(signature->id());
-				Signature tmpTo = _memory->getSignatureData(_loopClosureHypothesis.first, true);
+				SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
-				SensorData dataTo = tmpTo.toSensorData();
 				UDEBUG("timeTo = %fs", timeT.ticks());
 
-				if(dataFrom.isValid() &&
+				if(!dataFrom.depthOrRightRaw().empty() &&
-				   dataFrom.isMetric() &&
+				   !dataTo.depthOrRightRaw().empty() &&
-				   dataTo.isValid() &&
-				   dataTo.isMetric() &&
 				   dataFrom.id() != Memory::kIdInvalid &&
-				   tmpTo.id() != Memory::kIdInvalid)
+				   dataTo.id() != Memory::kIdInvalid)
 				{
 					memory.update(dataTo);
 					UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1683,7 +1686,7 @@ bool Rtabmap::process(const SensorData & data)
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
 		}
 
 		if(rejectedHypothesis)
@@ -1797,16 +1800,13 @@ bool Rtabmap::process(const SensorData & data)
 							// Add signatures
 							SensorData dataFrom = data;
 							dataFrom.setId(signature->id());
-							Signature tmpTo = _memory->getSignatureData(nearestId, true);
+							SensorData dataTo = _memory->getNodeData(nearestId, true);
-							SensorData dataTo = tmpTo.toSensorData();
 							UDEBUG("timeTo = %fs", timeT.ticks());
 
-							if(dataFrom.isValid() &&
+							if(!dataFrom.depthOrRightRaw().empty() &&
-							   dataFrom.isMetric() &&
+							   !dataTo.depthOrRightRaw().empty() &&
-							   dataTo.isValid() &&
-							   dataTo.isMetric() &&
 							   dataFrom.id() != Memory::kIdInvalid &&
-							   tmpTo.id() != Memory::kIdInvalid)
+							   dataTo.id() != Memory::kIdInvalid)
 							{
 								memory.update(dataTo);
 								UDEBUG("timeUpTo = %fs", timeT.ticks());
@@ -1838,7 +1838,7 @@ bool Rtabmap::process(const SensorData & data)
 									signature->id(),
 									nearestId,
 									transform.prettyPrint().c_str());
-							_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
+							_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
 
 							if(_loopClosureHypothesis.first == 0)
 							{
@@ -1856,7 +1856,7 @@ bool Rtabmap::process(const SensorData & data)
 				//
 				// 2) compare locally with nearest locations by scan matching
 				//
-				if( !signature->getLaserScanCompressed().empty() &&
+				if( !signature->sensorData().laserScanCompressed().empty() &&
 					(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
 				{
 					// In localization mode, no need to check local loop
@@ -1927,7 +1927,7 @@ bool Rtabmap::process(const SensorData & data)
 												nearestId,
 												transform.prettyPrint().c_str());
 										// set Identify covariance for laser scan matching only
-										_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
+										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
 
 										++localSpaceClosuresAddedByICPOnly;
 
@@ -1967,6 +1967,7 @@ bool Rtabmap::process(const SensorData & data)
 			UINFO("Update map correction: SLAM mode");
 			// SLAM mode!
 			optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
+			UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
 
 			// Update map correction, it should be identify when optimizing from the last node
 			_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
@@ -2015,7 +2016,7 @@ bool Rtabmap::process(const SensorData & data)
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
 			if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
 			{
-				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance
+				_memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance
 			}
 		}
 	}
@@ -2085,44 +2086,6 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.setMapCorrection(_mapCorrection);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 
-			// Set local graph
-			if(!_rgbdSlamMode)
-			{
-				// no optimization on appearance-only mode, create a local graph
-				std::map<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
-				std::map<int, Transform> poses;
-				std::map<int, int> mapIds;
-				std::map<int, std::string> labels;
-				std::map<int, double> stamps;
-				std::map<int, std::vector<unsigned char> > userDatas;
-				std::multimap<int, Link> constraints;
-				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
-				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					Transform odomPose;
-					int weight = -1;
-					int mapId = -1;
-					std::string label;
-					double stamp = 0;
-					std::vector<unsigned char> userData;
-					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
-					mapIds.insert(std::make_pair(iter->first, mapId));
-					labels.insert(std::make_pair(iter->first, label));
-					stamps.insert(std::make_pair(iter->first, stamp));
-					userDatas.insert(std::make_pair(iter->first, userData));
-				}
-				statistics_.setPoses(poses);
-				statistics_.setConstraints(constraints);
-				statistics_.setMapIds(mapIds);
-				statistics_.setLabels(labels);
-				statistics_.setStamps(stamps);
-				statistics_.setUserDatas(userDatas);
-			}
-			else // RGBD-SLAM mode
-			{
-				//see after transfer below
-			}
-
 			// timings...
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
@@ -2146,11 +2109,6 @@ bool Rtabmap::process(const SensorData & data)
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
-			if(_publishLastSignature)
-			{
-				statistics_.setSignature(*signature);
-			}
-
 			if(_publishLikelihood || _publishPdf)
 			{
 				// Child count by parent signature on the root of the memory ... for statistics
@@ -2178,6 +2136,12 @@ bool Rtabmap::process(const SensorData & data)
 		ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
 	}
 
+	Signature lastSignatureData(signature->id());
+	if(_publishLastSignatureData)
+	{
+		lastSignatureData = *signature;
+	}
+
 	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
 	//This will also remove rehearsed signatures
 	std::list<int> signaturesRemoved = _memory->cleanup();
@@ -2206,11 +2170,13 @@ bool Rtabmap::process(const SensorData & data)
 		_memory->deleteLocation(signature->id());
 	}
 
-	timeMemoryCleanup = timer.ticks();
-	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
-
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;
+
+	timeMemoryCleanup = timer.ticks();
+	ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size());
+
+
 
 	//============================================================
 	// TRANSFER
@@ -2275,6 +2241,7 @@ bool Rtabmap::process(const SensorData & data)
 	//==============================================================
 	// Finalize statistics and log files
 	//==============================================================
+	int localGraphSize = 0;
 	if(_publishStats)
 	{
 		statistics_.addStatistic(Statistics::kTimingStatistics_creation(), timeStatsCreation*1000);
@@ -2293,36 +2260,49 @@ bool Rtabmap::process(const SensorData & data)
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
-		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());
 
-		if(_rgbdSlamMode)
+		std::map<int, Signature> signatures;
+		if(_publishLastSignatureData)
 		{
-			std::map<int, int> mapIds;
+			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
-			std::map<int, std::string> labels;
-			std::map<int, double> stamps;
-			std::map<int, std::vector<unsigned char> > userDatas;
-			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-			{
-				Transform odomPose;
-				int weight = -1;
-				int mapId = -1;
-				std::string label;
-				double stamp = 0;
-				std::vector<unsigned char> userData;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
-				mapIds.insert(std::make_pair(iter->first, mapId));
-				labels.insert(std::make_pair(iter->first, label));
-				stamps.insert(std::make_pair(iter->first, stamp));
-				userDatas.insert(std::make_pair(iter->first, userData));
-			}
-			statistics_.setPoses(_optimizedPoses);
-			statistics_.setConstraints(_constraints);
-			statistics_.setMapIds(mapIds);
-			statistics_.setLabels(labels);
-			statistics_.setStamps(stamps);
-			statistics_.setUserDatas(userDatas);
 		}
-
+		// Set local graph
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+		if(!_rgbdSlamMode)
+		{
+			// no optimization on appearance-only mode, create a local graph
+			std::map<int, int> ids = _memory->getNeighborsId(lastSignatureData.id(), 0, 0, true);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
+		}
+		else // RGBD-SLAM mode
+		{
+			poses = _optimizedPoses;
+			constraints = _constraints;
+		}
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			Transform odomPose;
+			int weight = -1;
+			int mapId = -1;
+			std::string label;
+			double stamp = 0;
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
+			signatures.insert(std::make_pair(iter->first,
+					Signature(iter->first,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
+		}
+		statistics_.setPoses(poses);
+		statistics_.setConstraints(constraints);
+		statistics_.setSignatures(signatures);
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
+		localGraphSize = poses.size();
 	}
 
 	//Start trashing
@@ -2359,7 +2339,7 @@ bool Rtabmap::process(const SensorData & data)
 									timeLocalTimeDetection,
 									timeLocalSpaceDetection,
 									timeMapOptimization);
-		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
+		std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
 									_loopClosureHypothesis.first,
 									_highestHypothesis.first,
 									(int)signaturesRemoved.size(),
@@ -2374,9 +2354,11 @@ bool Rtabmap::process(const SensorData & data)
 									lcHypothesisReactivated,
 									refUniqueWordsCount,
 									retrievalId,
-									0.0f,
+									0,
 									rehearsalMaxId,
-									rehearsalMaxId>0?1:0);
+									rehearsalMaxId>0?1:0,
+									localGraphSize,
+									data.id());
 		if(_statisticLogsBufferedInRAM)
 		{
 			_bufferedLogsF.push_back(logF);
@@ -2403,7 +2385,7 @@ bool Rtabmap::process(const SensorData & data)
 
 bool Rtabmap::process(const cv::Mat & image, int id)
 {
-	return this->process(SensorData(image, id));
+	return this->process(SensorData(image, id), Transform());
 }
 
 // SETTERS
@@ -2838,13 +2820,10 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
-void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
+void Rtabmap::get3DMap(
+		std::map<int, Signature> & signatures,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
 		bool global) const
 {
@@ -2870,22 +2849,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-		{
-			Transform odomPose;
-			int weight = -1;
-			int mapId = -1;
-			std::string label;
-			double stamp = 0;
-			std::vector<unsigned char> userData;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
-			mapIds.insert(std::make_pair(iter->first, mapId));
-			stamps.insert(std::make_pair(iter->first, stamp));
-			labels.insert(std::make_pair(iter->first, label));
-			userDatas.insert(std::make_pair(iter->first, userData));
-		}
-
-
 		// Get data
 		std::set<int> ids = uKeysSet(_memory->getWorkingMem()); // WM
 
@@ -2900,11 +2863,24 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			Signature data = _memory->getSignatureData(*iter);
+			Transform odomPose;
-			if(data.id() != Memory::kIdInvalid)
+			int weight = -1;
-			{
+			int mapId = -1;
-				signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
+			std::string label;
-			}
+			double stamp = 0;
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true);
+			SensorData data = _memory->getNodeData(*iter);
+			data.setId(*iter);
+			signatures.insert(std::make_pair(*iter,
+					Signature(*iter,
+							mapId,
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData,
+							data)));
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -2920,13 +2896,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
-		bool global,
+		bool global,
-		bool posesConstraintsOnly)
+		std::map<int, Signature> * signatures)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -2948,8 +2920,8 @@ void Rtabmap::getGraph(
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
-
+
-		if(!posesConstraintsOnly)
+		if(signatures)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -2958,12 +2930,16 @@ void Rtabmap::getGraph(
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				std::vector<unsigned char> userData;
+				std::vector<unsigned char> userData;
 				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
-				mapIds.insert(std::make_pair(iter->first, mapId));
+				signatures->insert(std::make_pair(iter->first,
-				stamps.insert(std::make_pair(iter->first, stamp));
+						Signature(iter->first,
-				labels.insert(std::make_pair(iter->first, label));
+							mapId,
-				userDatas.insert(std::make_pair(iter->first, userData));
+							weight,
+							stamp,
+							label,
+							odomPose,
+							userData)));
 			}
 		}
 	}
@@ -3115,12 +3091,8 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	UTimer totalTimer;
 	UTimer timer;
 	std::map<int, Transform> nodes;
-	std::multimap<int, Link> constraints;
+	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
+	this->getGraph(nodes, constraints, true, global);
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global, true);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	if(computePath(targetNode, nodes, constraints))
@@ -3153,8 +3125,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	std::map<int, int> mapIds;
 	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
+	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global, true);
+	this->getGraph(nodes, constraints, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
@@ -3306,7 +3278,7 @@ void Rtabmap::updateGoalIndex()
 						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
 							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance
+							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance
 							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}

corelib/src/RtabmapThread.cpp

@@ -130,20 +130,13 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	_rtabmap->get3DMap(signatures,
 			poses,
 			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
 			optimized,
 			full);
 
-	this->post(new RtabmapEvent3DMap(signatures,
+	this->post(new RtabmapEvent3DMap(
+			signatures,
 			poses,
-			constraints,
+			constraints));
-			mapIds,
-			stamps,
-			labels,
-			userDatas));
 }
 
 void RtabmapThread::publishGraph(bool optimized, bool full) const
@@ -158,20 +151,14 @@ void RtabmapThread::publishGraph(bool optimized, bool full) const
 
 	_rtabmap->getGraph(poses,
 			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
 			optimized,
-			full);
+			full,
+			&signatures);
 
-	this->post(new RtabmapEvent3DMap(signatures,
+	this->post(new RtabmapEvent3DMap(
+			signatures,
 			poses,
-			constraints,
+			constraints));
-			mapIds,
-			stamps,
-			labels,
-			userDatas));
 }
 
 
@@ -314,16 +301,16 @@ void RtabmapThread::handleEvent(UEvent* event)
 		CameraEvent * e = (CameraEvent*)event;
 		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
 		{
-			this->addData(e->data());
+			this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
 		}
 	}
 	else if(event->getClassName().compare("OdometryEvent") == 0)
 	{
 		UDEBUG("OdometryEvent");
 		OdometryEvent * e = (OdometryEvent*)event;
-		if(e->isValid())
+		if(!e->pose().isNull())
 		{
-			this->addData(e->data());
+			this->addData(*e);
 		}
 		else
 		{
@@ -522,12 +509,12 @@ void RtabmapThread::handleEvent(UEvent* event)
 //============================================================
 void RtabmapThread::process()
 {
-	SensorData data;
+	OdometryEvent data;
 	if(_state.empty() && getData(data))
 	{
 		if(_rtabmap->getMemory())
 		{
-			if(_rtabmap->process(data))
+			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -542,16 +529,10 @@ void RtabmapThread::process()
 	}
 }
 
-void RtabmapThread::addData(const SensorData & sensorData)
+void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
-		if(!sensorData.isValid())
-		{
-			ULOGGER_ERROR("data not valid !?");
-			return;
-		}
-
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
@@ -559,9 +540,8 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			{
 				ignoreFrame = true;
 			}
-
 		}
-		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity())
+		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity())
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
@@ -578,48 +558,45 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			_frameRateTimer->start();
 		}
 
-		lastPose_ = sensorData.pose();
+		lastPose_ = odomEvent.pose();
-		if(sensorData.poseRotVariance() > _rotVariance)
+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		if(maxRotVar > _rotVariance)
 		{
-			_rotVariance = sensorData.poseRotVariance();
+			_rotVariance = maxRotVar;
 		}
-		if(sensorData.poseTransVariance() > _transVariance)
+		if(maxTransVar > _transVariance)
 		{
-			_transVariance = sensorData.poseTransVariance();
+			_transVariance = maxTransVar;
 		}
 
 		bool notify = true;
 		_dataMutex.lock();
 		{
+			if(_rotVariance <= 0)
+			{
+				_rotVariance = 1.0;
+			}
+			if(_transVariance <= 0)
+			{
+				_transVariance = 1.0;
+			}
 			if(ignoreFrame)
 			{
 				// remove data from the frame, keeping only constraints
 				SensorData tmp(
 						cv::Mat(),
-						cv::Mat(),
+						odomEvent.data().id(),
-						0,0,0,0,
+						odomEvent.data().stamp(),
-						sensorData.localTransform(),
+						odomEvent.data().userData());
-						sensorData.pose(),
+				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
-						sensorData.poseRotVariance(),
-						sensorData.poseTransVariance(),
-						sensorData.id(),
-						sensorData.stamp(),
-						sensorData.userData());
-				_dataBuffer.push_back(tmp);
 			}
 			else
 			{
-				_dataBuffer.push_back(sensorData);
+				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
 			}
-			if(_rotVariance <= 0)
+			UDEBUG("Added data %d", odomEvent.data().id());
-			{
+
-				_rotVariance = 1.0f;
-			}
-			if(_transVariance <= 0)
-			{
-				_transVariance = 1.0f;
-			}
-			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
 			_rotVariance = 0;
 			_transVariance = 0;
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
@@ -638,7 +615,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 	}
 }
 
-bool RtabmapThread::getData(SensorData & image)
+bool RtabmapThread::getData(OdometryEvent & data)
 {
 	ULOGGER_DEBUG("");
 
@@ -651,7 +628,7 @@ bool RtabmapThread::getData(SensorData & image)
 	{
 		if(!_dataBuffer.empty())
 		{
-			image = _dataBuffer.front();
+			data = _dataBuffer.front();
 			_dataBuffer.pop_front();
 			dataFilled = true;
 		}

corelib/src/SensorData.cpp

@@ -27,138 +27,430 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <rtabmap/utilite/UMath.h>
 
 namespace rtabmap
 {
 
-/**
+// empty constructor
- * An id is automatically generated if id=0.
- */
 SensorData::SensorData() :
-	_id(0),
+		_id(0),
-	_stamp(0.0),
+		_stamp(0.0),
-	_fx(0.0f),
+		_laserScanMaxPts(0)
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0)
 {
 }
 
-SensorData::SensorData(const cv::Mat & image,
+// Appearance-only constructor
-	  int id,
+SensorData::SensorData(
-	  double stamp,
+		const cv::Mat & image,
-	  const std::vector<unsigned char> & userData) :
+		int id,
-	_image(image),
+		double stamp,
-	_id(id),
+		const std::vector<unsigned char> & userData) :
-	_stamp(stamp),
+		_id(id),
-	_fx(0.0f),
+		_stamp(stamp),
-	_fyOrBaseline(0.0f),
+		_laserScanMaxPts(0),
-	_cx(0.0f),
+		_userData(userData)
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0),
-	_userData(userData)
 {
-	UASSERT(image.empty() ||
+	if(image.rows == 1)
-			image.type() == CV_8UC1 || // Mono
+	{
-			image.type() == CV_8UC3);  // RGB
+		UASSERT(image.type() == CV_8UC1); // Bytes
+		_imageCompressed = image;
+	}
+	else if(!image.empty())
+	{
+		UASSERT(image.type() == CV_8UC1 || // Mono
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
 }
 
-	// Metric constructor
+// Mono constructor
-SensorData::SensorData(const cv::Mat & image,
+SensorData::SensorData(
-		  const cv::Mat & depthOrRightImage,
+		const cv::Mat & image,
-		  float fx,
+		const CameraModel & cameraModel,
-		  float fyOrBaseline,
+		int id,
-		  float cx,
+		double stamp,
-		  float cy,
+		const std::vector<unsigned char> & userData) :
-		  const Transform & localTransform,
+		_id(id),
-		  const Transform & pose,
+		_stamp(stamp),
-		  float poseRotVariance,
+		_laserScanMaxPts(0),
-		  float poseTransVariance,
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
-		  int id,
+		_userData(userData)
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(0),
-	_userData(userData)
 {
-	UASSERT(image.empty() ||
+	if(image.rows == 1)
-			image.type() == CV_8UC1 || // Mono
+	{
-			image.type() == CV_8UC3);  // RGB
+		UASSERT(image.type() == CV_8UC1); // Bytes
-	UASSERT(depthOrRightImage.empty() ||
+		_imageCompressed = image;
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+	}
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+	else if(!image.empty())
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
+	{
-	UASSERT(!_localTransform.isNull());
+		UASSERT(image.type() == CV_8UC1 || // Mono
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
 }
 
-	// Metric constructor + 2d depth
+// RGB-D constructor
-SensorData::SensorData(const cv::Mat & laserScan,
+SensorData::SensorData(
-		  int laserScanMaxPts,
+		const cv::Mat & rgb,
-		  const cv::Mat & image,
+		const cv::Mat & depth,
-		  const cv::Mat & depthOrRightImage,
+		const CameraModel & cameraModel,
-		  float fx,
+		int id,
-		  float fyOrBaseline,
+		double stamp,
-		  float cx,
+		const std::vector<unsigned char> & userData) :
-		  float cy,
+		_id(id),
-		  const Transform & localTransform,
+		_stamp(stamp),
-		  const Transform & pose,
+		_laserScanMaxPts(0),
-		  float poseRotVariance,
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
-		  float poseTransVariance,
+		_userData(userData)
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_laserScan(laserScan),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(laserScanMaxPts),
-	_userData(userData)
 {
-	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
+	if(rgb.rows == 1)
-	UASSERT(image.empty() ||
+	{
-			image.type() == CV_8UC1 || // Mono
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
-			image.type() == CV_8UC3);  // RGB
+		_imageCompressed = rgb;
-	UASSERT(depthOrRightImage.empty() ||
+	}
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+	else if(!rgb.empty())
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+	{
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
-	UASSERT(!_localTransform.isNull());
+				rgb.type() == CV_8UC3);  // RGB
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+		_imageRaw = rgb;
+	}
+
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
 }
 
-bool SensorData::empty() const
+// RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_userData(userData)
 {
-	return _image.empty();
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+}
+
+// Multi-cameras RGB-D constructor
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_cameraModels(cameraModels),
+		_userData(userData)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+	for(unsigned int i=0; i<cameraModels.size(); ++i)
+	{
+		UASSERT(cameraModels[i].isValid());
+	}
+}
+
+// Multi-cameras RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(cameraModels),
+		_userData(userData)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+
+	for(unsigned int i=0; i<cameraModels.size(); ++i)
+	{
+		UASSERT(cameraModels[i].isValid());
+	}
+}
+
+// Stereo constructor
+SensorData::SensorData(
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData):
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_stereoCameraModel(cameraModel),
+		_userData(userData)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+}
+
+// Stereo constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const std::vector<unsigned char> & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_stereoCameraModel(cameraModel),
+		_userData(userData)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+}
+
+void SensorData::uncompressData()
+{
+	uncompressData(_imageCompressed.empty()?0:&_imageRaw,
+				_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
+				_laserScanCompressed.empty()?0:&_laserScanRaw);
+}
+
+void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
+{
+	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
+	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
+	{
+		_imageRaw = *imageRaw;
+	}
+	if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
+	{
+		_depthOrRightRaw = *depthRaw;
+	}
+	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
+	{
+		_laserScanRaw = *laserScanRaw;
+	}
+}
+
+void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
+{
+	if(imageRaw)
+	{
+		*imageRaw = _imageRaw;
+	}
+	if(depthRaw)
+	{
+		*depthRaw = _depthOrRightRaw;
+	}
+	if(laserScanRaw)
+	{
+		*laserScanRaw = _laserScanRaw;
+	}
+	if( (imageRaw && imageRaw->empty()) ||
+		(depthRaw && depthRaw->empty()) ||
+		(laserScanRaw && laserScanRaw->empty()))
+	{
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
+		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		if(imageRaw && imageRaw->empty())
+		{
+			ctImage.start();
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			ctDepth.start();
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			ctLaserScan.start();
+		}
+		ctImage.join();
+		ctDepth.join();
+		ctLaserScan.join();
+		if(imageRaw && imageRaw->empty())
+		{
+			*imageRaw = ctImage.getUncompressedData();
+			if(imageRaw->empty())
+			{
+				UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
+			}
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			*depthRaw = ctDepth.getUncompressedData();
+			if(depthRaw->empty())
+			{
+				UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
+			}
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			*laserScanRaw = ctLaserScan.getUncompressedData();
+
+			if(laserScanRaw->empty())
+			{
+				UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
+			}
+		}
+	}
 }
 
 } // namespace rtabmap

corelib/src/Signature.cpp

@@ -39,17 +39,11 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_stamp(0.0),
+	_weight(0),
-	_weight(-1),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false),
+	_enabled(false)
-	_fx(0.0f),
-	_fy(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_laserScanMaxPts(0)
 {
 }
 
@@ -59,19 +53,9 @@ Signature::Signature(
 		int weight,
 		double stamp,
 		const std::string & label,
-		const std::multimap<int, cv::KeyPoint> & words,
-		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
 		const std::vector<unsigned char> & userData,
-		const cv::Mat & laserScanCompressed, // in base_link frame
+		const SensorData & sensorData):
-		const cv::Mat & imageCompressed, // in camera_link frame
-		const cv::Mat & depthCompressed, // in camera_link frame
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		int laserScanMaxPts) :
 	_id(id),
 	_mapId(mapId),
 	_stamp(stamp),
@@ -81,20 +65,15 @@ Signature::Signature(
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_words(words),
 	_enabled(false),
-	_imageCompressed(imageCompressed),
-	_depthCompressed(depthCompressed),
-	_laserScanCompressed(laserScanCompressed),
-	_fx(fx),
-	_fy(fy),
-	_cx(cx),
-	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform),
+	_sensorData(sensorData)
-	_words3(words3),
-	_laserScanMaxPts(laserScanMaxPts)
 {
+	if(_sensorData.id() == 0)
+	{
+		_sensorData.setId(id);
+	}
+	UASSERT(_sensorData.id() == _id);
 }
 
 Signature::~Signature()
@@ -239,25 +218,9 @@ void Signature::removeWord(int wordId)
 	_words3.erase(wordId);
 }
 
-void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
+cv::Mat Signature::getPoseCovariance() const
 {
-	UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-	_depthCompressed = bytes;
-	_fx=fx;
-	_fy=fy;
-	_cx=cx;
-	_cy=cy;
-}
-
-float Signature::getDepthFx() const {return getFx();}
-float Signature::getDepthFy() const {return getFy();}
-float Signature::getDepthCx() const {return getCx();}
-float Signature::getDepthCy() const {return getCy();}
-
-void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
-{
-	rotVariance = 1.0f;
-	transVariance = 1.0f;
 	if(_links.size())
 	{
 		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -267,110 +230,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons
 				//Assume the first neighbor to be the backward neighbor link
 				if(iter->second.to() < iter->second.from())
 				{
-					rotVariance = iter->second.rotVariance();
+					covariance = iter->second.infMatrix().inv();
-					transVariance = iter->second.transVariance();
 					break;
 				}
 			}
 		}
 	}
-}
+	return covariance;
-
-SensorData Signature::toSensorData()
-{
-	this->uncompressData();
-	float rotVariance = 1.0f;
-	float transVariance = 1.0f;
-	this->getPoseVariance(rotVariance, transVariance);
-
-	return SensorData(_laserScanRaw,
-			_laserScanMaxPts,
-			_imageRaw,
-			_depthRaw,
-			_fx,
-			_fy,
-			_cx,
-			_cy,
-			_localTransform,
-			_pose,
-			rotVariance,
-			transVariance,
-			_id,
-			_stamp,
-			_userData);
-}
-
-void Signature::uncompressData()
-{
-	uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
-}
-
-void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
-{
-	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
-	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
-	{
-		_imageRaw = *imageRaw;
-	}
-	if(depthRaw && !depthRaw->empty() && _depthRaw.empty())
-	{
-		_depthRaw = *depthRaw;
-	}
-	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
-	{
-		_laserScanRaw = *laserScanRaw;
-	}
-}
-
-void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
-{
-	if(imageRaw)
-	{
-		*imageRaw = _imageRaw;
-	}
-	if(depthRaw)
-	{
-		*depthRaw = _depthRaw;
-	}
-	if(laserScanRaw)
-	{
-		*laserScanRaw = _laserScanRaw;
-	}
-	if( (imageRaw && imageRaw->empty()) ||
-		(depthRaw && depthRaw->empty()) ||
-		(laserScanRaw && laserScanRaw->empty()))
-	{
-		rtabmap::CompressionThread ctImage(_imageCompressed, true);
-		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
-		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
-		if(imageRaw && imageRaw->empty())
-		{
-			ctImage.start();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			ctDepth.start();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			ctLaserScan.start();
-		}
-		ctImage.join();
-		ctDepth.join();
-		ctLaserScan.join();
-		if(imageRaw && imageRaw->empty())
-		{
-			*imageRaw = ctImage.getUncompressedData();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			*depthRaw = ctDepth.getUncompressedData();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			*laserScanRaw = ctLaserScan.getUncompressedData();
-		}
-	}
 }
 
 } //namespace rtabmap

corelib/src/Transform.cpp

@@ -31,44 +31,33 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <iomanip>
 
 namespace rtabmap {
 
-Transform::Transform() : data_(12)
+Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
 {
-	data_[0] = 0.0f;
-	data_[1] = 0.0f;
-	data_[2] = 0.0f;
-	data_[3] = 0.0f;
-	data_[4] = 0.0f;
-	data_[5] = 0.0f;
-	data_[6] = 0.0f;
-	data_[7] = 0.0f;
-	data_[8] = 0.0f;
-	data_[9] = 0.0f;
-	data_[10] = 0.0f;
-	data_[11] = 0.0f;
 }
 
 // rotation matrix r## and origin o##
-Transform::Transform(float r11, float r12, float r13, float o14,
+Transform::Transform(
-				     float r21, float r22, float r23, float o24,
+		float r11, float r12, float r13, float o14,
-				     float r31, float r32, float r33, float o34) :
+		float r21, float r22, float r23, float o24,
-	data_(12)
+		float r31, float r32, float r33, float o34)
 {
-	data_[0] = r11;
+	data_ = (cv::Mat_<float>(3,4) <<
-	data_[1] = r12;
+			r11, r12, r13, o14,
-	data_[2] = r13;
+			r21, r22, r23, o24,
-	data_[3] = o14;
+			r31, r32, r33, o34);
-	data_[4] = r21;
+}
-	data_[5] = r22;
+
-	data_[6] = r23;
+Transform::Transform(const cv::Mat & transformationMatrix)
-	data_[7] = o24;
+{
-	data_[8] = r31;
+	UASSERT(transformationMatrix.cols == 4 &&
-	data_[9] = r32;
+			transformationMatrix.rows == 3 &&
-	data_[10] = r33;
+			transformationMatrix.type() == CV_32FC1);
-	data_[11] = o34;
+	data_ = transformationMatrix;
 }
 
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
@@ -79,46 +68,46 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 
 bool Transform::isNull() const
 {
-	return (data_[0] == 0.0f &&
+	return (data()[0] == 0.0f &&
-			data_[1] == 0.0f &&
+			data()[1] == 0.0f &&
-			data_[2] == 0.0f &&
+			data()[2] == 0.0f &&
-			data_[3] == 0.0f &&
+			data()[3] == 0.0f &&
-			data_[4] == 0.0f &&
+			data()[4] == 0.0f &&
-			data_[5] == 0.0f &&
+			data()[5] == 0.0f &&
-			data_[6] == 0.0f &&
+			data()[6] == 0.0f &&
-			data_[7] == 0.0f &&
+			data()[7] == 0.0f &&
-			data_[8] == 0.0f &&
+			data()[8] == 0.0f &&
-			data_[9] == 0.0f &&
+			data()[9] == 0.0f &&
-			data_[10] == 0.0f &&
+			data()[10] == 0.0f &&
-			data_[11] == 0.0f) ||
+			data()[11] == 0.0f) ||
-			uIsNan(data_[0]) ||
+			uIsNan(data()[0]) ||
-			uIsNan(data_[1]) ||
+			uIsNan(data()[1]) ||
-			uIsNan(data_[2]) ||
+			uIsNan(data()[2]) ||
-			uIsNan(data_[3]) ||
+			uIsNan(data()[3]) ||
-			uIsNan(data_[4]) ||
+			uIsNan(data()[4]) ||
-			uIsNan(data_[5]) ||
+			uIsNan(data()[5]) ||
-			uIsNan(data_[6]) ||
+			uIsNan(data()[6]) ||
-			uIsNan(data_[7]) ||
+			uIsNan(data()[7]) ||
-			uIsNan(data_[8]) ||
+			uIsNan(data()[8]) ||
-			uIsNan(data_[9]) ||
+			uIsNan(data()[9]) ||
-			uIsNan(data_[10]) ||
+			uIsNan(data()[10]) ||
-			uIsNan(data_[11]);
+			uIsNan(data()[11]);
 }
 
 bool Transform::isIdentity() const
 {
-	return data_[0] == 1.0f &&
+	return data()[0] == 1.0f &&
-			data_[1] == 0.0f &&
+			data()[1] == 0.0f &&
-			data_[2] == 0.0f &&
+			data()[2] == 0.0f &&
-			data_[3] == 0.0f &&
+			data()[3] == 0.0f &&
-			data_[4] == 0.0f &&
+			data()[4] == 0.0f &&
-			data_[5] == 1.0f &&
+			data()[5] == 1.0f &&
-			data_[6] == 0.0f &&
+			data()[6] == 0.0f &&
-			data_[7] == 0.0f &&
+			data()[7] == 0.0f &&
-			data_[8] == 0.0f &&
+			data()[8] == 0.0f &&
-			data_[9] == 0.0f &&
+			data()[9] == 0.0f &&
-			data_[10] == 1.0f &&
+			data()[10] == 1.0f &&
-			data_[11] == 0.0f;
+			data()[11] == 0.0f;
 }
 
 void Transform::setNull()
@@ -145,16 +134,17 @@ Transform Transform::inverse() const
 
 Transform Transform::rotation() const
 {
-	return Transform(data_[0], data_[1], data_[2], 0,
+	return Transform(
-					 data_[4], data_[5], data_[6], 0,
+			data()[0], data()[1], data()[2], 0,
-					 data_[8], data_[9], data_[10], 0);
+			data()[4], data()[5], data()[6], 0,
+			data()[8], data()[9], data()[10], 0);
 }
 
 Transform Transform::translation() const
 {
-	return Transform(1,0,0, data_[3],
+	return Transform(1,0,0, data()[3],
-					 0,1,0, data_[7],
+					 0,1,0, data()[7],
-					 0,0,1, data_[11]);
+					 0,0,1, data()[11]);
 }
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
@@ -215,7 +205,7 @@ Transform & Transform::operator*=(const Transform & t)
 
 bool Transform::operator==(const Transform & t) const
 {
-	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+	return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
 }
 
 bool Transform::operator!=(const Transform & t) const
@@ -239,18 +229,18 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 Eigen::Matrix4f Transform::toEigen4f() const
 {
 	Eigen::Matrix4f m;
-	m << data_[0], data_[1], data_[2], data_[3],
+	m << data()[0], data()[1], data()[2], data()[3],
-		 data_[4], data_[5], data_[6], data_[7],
+		 data()[4], data()[5], data()[6], data()[7],
-		 data_[8], data_[9], data_[10], data_[11],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }
 Eigen::Matrix4d Transform::toEigen4d() const
 {
 	Eigen::Matrix4d m;
-	m << data_[0], data_[1], data_[2], data_[3],
+	m << data()[0], data()[1], data()[2], data()[3],
-		 data_[4], data_[5], data_[6], data_[7],
+		 data()[4], data()[5], data()[6], data()[7],
-		 data_[8], data_[9], data_[10], data_[11],
+		 data()[8], data()[9], data()[10], data()[11],
 		 0,0,0,1;
 	return m;
 }

corelib/src/resources/DatabaseSchema.sql.in

@@ -25,24 +25,13 @@ CREATE TABLE Node (
 	PRIMARY KEY (id)
 );
 
-CREATE TABLE Image (
+CREATE TABLE Data (
 	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (RGB)
+	image BLOB,               -- compressed image (Grayscale or RGB)
-	time_enter DATE,
+	depth BLOB,               -- compressed image (Depth or Right image)
-	PRIMARY KEY (id)
+	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
-);
+	scan BLOB,                -- compressed data (Laser scan)
-
+	scan_max_pts INTEGER,     -- Laser scan max points
--- TODO: Merge "Image" and "Depth" tables to "Data" table.
-CREATE TABLE Depth (
-	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (Depth or Right image)
-	fx FLOAT,
-	fy FLOAT,        -- baseline if stereo
-	cx FLOAT,
-	cy FLOAT,
-	local_transform BLOB,
-	data2d BLOB,     -- compressed data (Laser scan)
-	data2d_max_pts INTEGER,   -- Laser scan max points
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

corelib/src/util3d.cpp

@@ -27,10 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap
@@ -494,6 +496,383 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			decimation);
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+	{
+		//depth
+		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+		cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+		{
+			if(sensorData.cameraModels()[i].isValid())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
+						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i].cy(),
+						sensorData.cameraModels()[i].fx(),
+						sensorData.cameraModels()[i].fy(),
+						decimation);
+
+				if(tmp->size())
+				{
+					bool filtered = false;
+					if(tmp->size() && maxDepth)
+					{
+						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+						filtered = true;
+					}
+
+					if(tmp->size() && voxelSize)
+					{
+						tmp = util3d::voxelize(tmp, voxelSize);
+						filtered = true;
+					}
+
+					if(tmp->size() && samples)
+					{
+						tmp = util3d::sampling(tmp, samples);
+						filtered = true;
+					}
+
+					if(tmp->size() && !filtered)
+					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
+					}
+
+					if(tmp->size())
+					{
+						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+					}
+
+					*cloud += *tmp;
+				}
+			}
+			else
+			{
+				UERROR("Camera model %d is invalid", i);
+			}
+		}
+
+		if(cloud->size() && voxelSize)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize);
+		}
+	}
+	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+	{
+		//stereo
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
+
+		cv::Mat leftMono;
+		if(sensorData.imageRaw().channels() == 3)
+		{
+			cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = sensorData.imageRaw();
+		}
+		cloud = cloudFromDisparity(
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				sensorData.stereoCameraModel().left().cx(),
+				sensorData.stereoCameraModel().left().cy(),
+				sensorData.stereoCameraModel().left().fx(),
+				sensorData.stereoCameraModel().baseline(),
+				decimation);
+
+		if(cloud->size())
+		{
+			bool filtered = false;
+			if(cloud->size() && maxDepth)
+			{
+				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+				filtered = true;
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+				filtered = true;
+			}
+
+			if(cloud->size() && !filtered)
+			{
+				cloud = util3d::removeNaNFromPointCloud(cloud);
+			}
+
+			if(cloud->size())
+			{
+				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+
+	if(!sensorData.imageRaw().empty())
+	{
+		if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+		{
+			//depth
+			UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
+			UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
+			int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+			cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+			{
+				if(sensorData.cameraModels()[i].isValid())
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
+							cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
+							cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+							sensorData.cameraModels()[i].cx(),
+							sensorData.cameraModels()[i].cy(),
+							sensorData.cameraModels()[i].fx(),
+							sensorData.cameraModels()[i].fy(),
+							decimation);
+
+					if(tmp->size())
+					{
+						bool filtered = false;
+						if(tmp->size() && maxDepth)
+						{
+							tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+							filtered = true;
+						}
+
+						if(tmp->size() && voxelSize)
+						{
+							tmp = util3d::voxelize(tmp, voxelSize);
+							filtered = true;
+						}
+
+						if(tmp->size() && samples)
+						{
+							tmp = util3d::sampling(tmp, samples);
+							filtered = true;
+						}
+
+						if(tmp->size() && !filtered)
+						{
+							tmp = util3d::removeNaNFromPointCloud(tmp);
+						}
+
+						if(tmp->size())
+						{
+							tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+						}
+
+						*cloud += *tmp;
+					}
+				}
+				else
+				{
+					UERROR("Camera model %d is invalid", i);
+				}
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+			}
+		}
+		else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+		{
+			//stereo
+			cloud = cloudFromStereoImages(sensorData.imageRaw(),
+					sensorData.rightRaw(),
+					sensorData.stereoCameraModel().left().cx(),
+					sensorData.stereoCameraModel().left().cy(),
+					sensorData.stereoCameraModel().left().fx(),
+					sensorData.stereoCameraModel().baseline(),
+					decimation);
+
+			if(cloud->size())
+			{
+				bool filtered = false;
+				if(cloud->size() && maxDepth)
+				{
+					cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+					filtered = true;
+				}
+
+				if(cloud->size() && voxelSize)
+				{
+					cloud = util3d::voxelize(cloud, voxelSize);
+					filtered = true;
+				}
+
+				if(cloud->size() && !filtered)
+				{
+					cloud = util3d::removeNaNFromPointCloud(cloud);
+				}
+
+				if(cloud->size())
+				{
+					cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+				}
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth,
+					const Transform & localTransform)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+	UASSERT(!localTransform.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ> scan;
+	int middle = depthImage.rows/2;
+	if(middle)
+	{
+		scan.resize(depthImage.cols);
+		int oi = 0;
+		for(int i=0; i<depthImage.cols; ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
+			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			{
+				if(!localTransform.isIdentity())
+				{
+					pt = util3d::transformPoint(pt, localTransform);
+				}
+				scan[oi++] = pt;
+			}
+		}
+		scan.resize(oi);
+	}
+	return scan;
+}
+
+
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+	}
+	return laserScan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	output->resize(laserScan.cols);
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+	}
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform)
+{
+	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
+	UASSERT(matrix.rows == 1);
+
+	Eigen::Affine3f t = tranform.toEigen3f();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(matrix.cols);
+	if(matrix.channels() == 2)
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
+			cloud->at(i).z = 0.0f;
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	else // channels=3
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
+			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	return cloud;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/util3d_conversions.cpp

@@ -1,142 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "rtabmap/core/util3d_conversions.h"
-
-#include "rtabmap/utilite/ULogger.h"
-#include <pcl/common/transforms.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
-cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
-{
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
-	for(unsigned int i=0; i<cloud.size(); ++i)
-	{
-		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
-		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
-	}
-	return laserScan;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
-{
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	output->resize(laserScan.cols);
-	for(int i=0; i<laserScan.cols; ++i)
-	{
-		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-	}
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform)
-{
-	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
-	UASSERT(matrix.rows == 1);
-
-	Eigen::Affine3f t = tranform.toEigen3f();
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(matrix.cols);
-	if(matrix.channels() == 2)
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
-			cloud->at(i).z = 0.0f;
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	else // channels=3
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
-			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	return cloud;
-}
-
-}
-
-}

corelib/src/util3d_features.cpp

@@ -44,36 +44,49 @@ namespace rtabmap
 namespace util3d
 {
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel)
+{
+	UASSERT(cameraModel.isValid());
+	std::vector<CameraModel> models;
+	models.push_back(cameraModel);
+	return generateKeypoints3DDepth(keypoints, depth, models);
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
+		const std::vector<CameraModel> & cameraModels)
-		float fy,
-		float cx,
-		float cy,
-		const Transform & transform)
 {
 	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
+	UASSERT(cameraModels.size());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	if(!depth.empty())
 	{
+		UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
+		float subImageWidth = depth.cols/cameraModels.size();
 		keypoints3d->resize(keypoints.size());
 		for(unsigned int i=0; i!=keypoints.size(); ++i)
 		{
+			int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
+			UASSERT(cameraIndex < (int)cameraModels.size());
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(
 					depth,
-					keypoints[i].pt.x,
+					keypoints[i].pt.x-subImageWidth*cameraIndex,
 					keypoints[i].pt.y,
-					cx,
+					cameraModels.at(cameraIndex).cx(),
-					cy,
+					cameraModels.at(cameraIndex).cy(),
-					fx,
+					cameraModels.at(cameraIndex).fx(),
-					fy,
+					cameraModels.at(cameraIndex).fy(),
 					true);
 
-			if(!transform.isNull() && !transform.isIdentity())
+			if(pcl::isFinite(pt) &&
+				!cameraModels.at(cameraIndex).localTransform().isNull() &&
+			    !cameraModels.at(cameraIndex).localTransform().isIdentity())
 			{
-				pt = util3d::transformPoint(pt, transform);
+				pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
 			}
 			keypoints3d->at(i) = pt;
 		}
@@ -84,13 +97,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		float fx,
+		const StereoCameraModel & stereoCameraModel)
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform)
 {
 	UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
+	UASSERT(stereoCameraModel.isValid());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	keypoints3d->resize(keypoints.size());
 	for(unsigned int i=0; i!=keypoints.size(); ++i)
@@ -98,14 +108,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		pcl::PointXYZ pt = util3d::projectDisparityTo3D(
 				keypoints[i].pt,
 				disparity,
-				cx,
+				stereoCameraModel.left().cx(),
-				cy,
+				stereoCameraModel.left().cy(),
-				fx,
+				stereoCameraModel.left().fx(),
-				baseline);
+				stereoCameraModel.baseline());
 
-		if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
+		if(pcl::isFinite(pt) &&
+			!stereoCameraModel.left().localTransform().isNull() &&
+			!stereoCameraModel.left().localTransform().isIdentity())
 		{
-			pt = util3d::transformPoint(pt, transform);
+			pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
 		}
 		keypoints3d->at(i) = pt;
 	}
@@ -120,7 +132,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform,
+		Transform localTransform,
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
@@ -137,7 +149,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 			baseline,
 			cx,
 			cy,
-			transform,
+			localTransform,
 			flowWinSize,
 			flowMaxLevel,
 			flowIterations,
@@ -153,7 +165,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform,
+		Transform localTransform,
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
@@ -163,6 +175,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 	UASSERT(!leftImage.empty() && !rightImage.empty() &&
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
 			leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
+	UASSERT(fx > 0.0f && baseline > 0.0f);
 
 	// Find features in the new left image
 	std::vector<unsigned char> status;
@@ -198,14 +211,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 				pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 						leftCorners[i],
 						disparity,
-						cx, cy, fx, baseline);
+						cx,
+						cy,
+						fx,
+						baseline);
 
 				if(pcl::isFinite(tmpPt))
 				{
 					pt = tmpPt;
-					if(!transform.isNull() && !transform.isIdentity())
+					if(!localTransform.isNull() &&
+					   !localTransform.isIdentity())
 					{
-						pt = util3d::transformPoint(pt, transform);
+						pt = util3d::transformPoint(pt, localTransform);
 					}
 				}
 			}
@@ -223,11 +240,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, cv::KeyPoint> & refWords,
 		const std::multimap<int, cv::KeyPoint> & nextWords,
-		float fx,
+		const CameraModel & cameraModel,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
 		Transform & cameraTransform,
 		int pnpIterations,
 		float pnpReprojError,
@@ -237,6 +250,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, pcl::PointXYZ> & refGuess3D,
 		double * varianceOut)
 {
+	UASSERT(cameraModel.isValid());
 	std::multimap<int, pcl::PointXYZ> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 	if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
@@ -290,10 +304,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					xp.at<double>(2, i) = 1;
 				}
 
-				cv::Mat K = (cv::Mat_<double>(3,3) <<
+				cv::Mat K = cameraModel.K();
-					fx, 0, cx,
-					0, fy, cy,
-					0, 0, 1);
 				cv::Mat Kinv = K.inv();
 				cv::Mat E = K.t()*F*K;
 				cv::Mat x_norm = Kinv * x;
@@ -313,7 +324,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					//if camera transform is set, use it instead of the computed one from epipolar geometry
 					if(useCameraTransformGuess)
 					{
-						Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
+						Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
 						P = (cv::Mat_<double>(3,4) <<
 								(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
 								(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
@@ -336,7 +347,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 						pts4D.col(i) /= pts4D.at<double>(3,i);
 						if(pts4D.at<double>(2,i) > 0)
 						{
-							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
+							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
 						}
 					}
 
@@ -349,7 +360,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
 									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
 
-						cameraTransform = (localTransform * t).inverse() * localTransform;
+						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
 					}
 
 					if(refGuess3D.size())
@@ -441,7 +452,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 								imagePoints.resize(oi);
 
 								//PnPRansac
-								Transform guess = localTransform.inverse();
+								Transform guess = cameraModel.localTransform().inverse();
 								cv::Mat R = (cv::Mat_<double>(3,3) <<
 										(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
 										(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -473,7 +484,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 												   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 												   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-									cameraTransform = (localTransform * pnp).inverse();
+									cameraTransform = (cameraModel.localTransform() * pnp).inverse();
 								}
 								else
 								{

corelib/src/util3d_mapping.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_mapping.h"
 
-#include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d.h>

examples/RGBDMapping/MapBuilder.h

@@ -71,8 +71,8 @@ public:
 		layout->addWidget(cloudViewer_);
 		this->setLayout(layout);
 
+		qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
-		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 
 		QAction * pause = new QAction(this);
 		this->addAction(pause);
@@ -102,14 +102,14 @@ protected slots:
 		}
 	}
 
-	virtual void processOdometry(const rtabmap::SensorData & data)
+	virtual void processOdometry(const rtabmap::OdometryEvent & odom)
 	{
 		if(!this->isVisible())
 		{
 			return;
 		}
 
-		Transform pose = data.pose();
+		Transform pose = odom.pose();
 		if(pose.isNull())
 		{
 			//Odometry lost
@@ -126,38 +126,33 @@ protected slots:
 			lastOdomPose_ = pose;
 
 			// 3d cloud
-			if(data.depth().cols == data.image().cols &&
+			if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
-			   data.depth().rows == data.image().rows &&
+			   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
-			   !data.depth().empty() &&
+			   !odom.data().depthOrRightRaw().empty() &&
-			   data.fx() > 0.0f &&
+			   (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
-			   data.fy() > 0.0f)
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
-					data.image(),
+					odom.data(),
-					data.depth(),
+					2,     // decimation
-					data.cx(),
+					4.0f); // max depth
-					data.cy(),
-					data.fx(),
-					data.fy(),
-					2); // decimation // high definition
 				if(cloud->size())
 				{
-					cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+					if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
-					if(cloud->size())
 					{
-						cloud = util3d::transformPointCloud(cloud, data.localTransform());
+						UERROR("Adding cloudOdom to viewer failed!");
 					}
 				}
-				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
+				else
 				{
-					UERROR("Adding cloudOdom to viewer failed!");
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					UWARN("Empty cloudOdom!");
 				}
 			}
 
-			if(!data.pose().isNull())
+			if(!odom.pose().isNull())
 			{
 				// update camera position
-				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*data.pose());
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
 			}
 		}
 		cloudViewer_->update();
@@ -196,35 +191,32 @@ protected slots:
 					}
 					cloudViewer_->setCloudVisibility(cloudName, true);
 				}
-				else if(iter->first == stats.refImageId() &&
+				else if(uContains(stats.getSignatures(), iter->first))
-						stats.getSignature().id() == iter->first)
 				{
-					Signature s = stats.getSignature();
+					Signature s = stats.getSignatures().at(iter->first);
-					s.uncompressData(); // make sure data is uncompressed
+					s.sensorData().uncompressData(); // make sure data is uncompressed
 					// Add the new cloud
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
-							s.getImageRaw(),
+							s.sensorData(),
-							s.getDepthRaw(),
+						    4,     // decimation
-							s.getCx(),
+						    4.0f); // max depth
-							s.getCy(),
-							s.getFx(),
-							s.getFy(),
-						   4); // decimation
-
 					if(cloud->size())
 					{
-						cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+						if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
-						if(cloud->size())
 						{
-							cloud = util3d::transformPointCloud(cloud, stats.getSignature().getLocalTransform());
+							UERROR("Adding cloud %d to viewer failed!", iter->first);
 						}
 					}
-					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
+					else
 					{
-						UERROR("Adding cloud %d to viewer failed!", iter->first);
+						UWARN("Empty cloud %d!", iter->first);
 					}
 				}
 			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
 		}
 
 		//============================
@@ -278,7 +270,7 @@ protected slots:
 			   !processingStatistics_)
 			{
 				lastOdometryProcessed_ = false; // if we receive too many odometry events!
-				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
 			}
 		}
 	}

examples/WifiMapping/MapBuilderWifi.h

@@ -78,26 +78,25 @@ protected slots:
 		std::map<double, int> nodeStamps; // <stamp, id>
 		std::map<int, std::pair<int, double> > wifiLevels;
 
-		UASSERT(stats.getStamps().size() == stats.getUserDatas().size());
+		for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
-		std::map<int, double>::const_iterator iterStamps = stats.getStamps().begin();
+			iter!=stats.getSignatures().end();
-		std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = stats.getUserDatas().begin();
+			++iter)
-		for(; iterStamps!=stats.getStamps().end() && iterUserDatas!=stats.getUserDatas().end(); ++iterStamps, ++iterUserDatas)
 		{
-			// Sort stamps by stamps
+			// Sort stamps by stamps->id
-			nodeStamps.insert(std::make_pair(iterStamps->second, iterStamps->first));
+			nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
 
 			// convert userData to wifi levels
-			if(iterUserDatas->second.size())
+			if(iter->second.getUserData().size())
 			{
-				UASSERT(iterUserDatas->second.size() == sizeof(int)+sizeof(double));
+				UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double));
 
 				// format [int level, double stamp]
 				int level;
 				double stamp;
-				memcpy(&level, iterUserDatas->second.data(), sizeof(int));
+				memcpy(&level, iter->second.getUserData().data(), sizeof(int));
-				memcpy(&stamp, iterUserDatas->second.data()+sizeof(int), sizeof(double));
+				memcpy(&stamp, iter->second.getUserData().data()+sizeof(int), sizeof(double));
 
-				wifiLevels.insert(std::make_pair(iterUserDatas->first, std::make_pair(level, stamp)));
+				wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp)));
 			}
 		}
 

guilib/include/rtabmap/gui/DataRecorder.h

@@ -57,7 +57,7 @@ public:
 	const QString & path() const {return path_;}
 
 public slots:
-	void addData(const rtabmap::SensorData & data);
+	void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1));
 	void showImage(const cv::Mat & image, const cv::Mat & depth);
 protected:
 	virtual void closeEvent(QCloseEvent* event);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -52,7 +52,7 @@ namespace rtabmap
 {
 class Memory;
 class ImageView;
-class Signature;
+class SensorData;
 class CloudViewer;
 
 class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
@@ -125,7 +125,7 @@ private:
 				QLabel * labelMapId,
 				QLabel * labelPose,
 				bool updateConstraintView);
-	void updateStereo(const Signature * data);
+	void updateStereo(const SensorData * data);
 	void updateWordsMatching();
 	void updateConstraintView(
 			const rtabmap::Link & link,

guilib/include/rtabmap/gui/MainWindow.h

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/gui/PreferencesDialog.h"
 
 #include <pcl/point_cloud.h>
@@ -163,7 +163,7 @@ private slots:
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
 	void updateElapsedTime();
-	void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
+	void processOdometry(const rtabmap::OdometryEvent & odom);
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
 	void processRtabmapEventInit(int status, const QString & info);
@@ -196,7 +196,7 @@ private slots:
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
-	void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &);
+	void odometryReceived(const rtabmap::OdometryEvent &);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);
@@ -229,19 +229,6 @@ private:
 			int regenerateDecimation,
 			float regenerateVoxelSize,
 			float regenerateMaxDepth) const;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
-			int id,
-			const cv::Mat & rgb,
-			const cv::Mat & depth,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			const Transform & localTransform,
-			const Transform & pose,
-			float voxelSize,
-			int decimation,
-			float maxDepth) const;
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
 			const std::map<int, Transform> & poses,
 			bool regenerateClouds,

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/OdometryEvent.h"
-#include "rtabmap/core/OdometryInfo.h"
 #include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"
 
@@ -59,7 +58,7 @@ protected:
 	virtual void handleEvent(UEvent * event);
 
 private slots:
-	void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
+	void processData(const rtabmap::OdometryEvent & odom);
 
 private:
 	ImageView* imageView_;

guilib/src/CalibrationDialog.cpp

@@ -239,8 +239,8 @@ void CalibrationDialog::handleEvent(UEvent * event)
 			{
 				processingData_ = true;
 				QMetaObject::invokeMethod(this, "processImages",
-						Q_ARG(cv::Mat, e->data().image()),
+						Q_ARG(cv::Mat, e->data().imageRaw()),
-						Q_ARG(cv::Mat, e->data().depthOrRightImage()),
+						Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
 						Q_ARG(QString, QString(e->cameraName().c_str())));
 			}
 		}

guilib/src/CameraViewer.cpp

@@ -76,19 +76,11 @@ CameraViewer::~CameraViewer()
 void CameraViewer::showImage(const rtabmap::SensorData & data)
 {
 	processingImages_ = true;
-	imageView_->setImage(uCvMat2QImage(data.image()));
+	imageView_->setImage(uCvMat2QImage(data.imageRaw()));
-	imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+	imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightRaw()));
-	if(!data.depth().empty() && data.fx() && data.fy())
+	if(!data.depthOrRightRaw().empty() && (data.stereoCameraModel().isValid() || data.cameraModels().size()))
 	{
-		cloudView_->addOrUpdateCloud("cloud",
+		cloudView_->addOrUpdateCloud("cloud", util3d::cloudFromSensorData(data));
-				util3d::cloudFromDepthRGB(data.image(), data.depth(), data.cx(), data.cy(), data.fx(), data.fy()),
-				data.localTransform());
-	}
-	else if(!data.rightImage().empty() && data.fx() && data.baseline())
-	{
-		cloudView_->addOrUpdateCloud("cloud",
-				util3d::cloudFromStereoImages(data.image(), data.rightImage(), data.cx(), data.cy(), data.fx(), data.baseline()),
-				data.localTransform());
 	}
 	else
 	{

guilib/src/CloudViewer.cpp

@@ -93,6 +93,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 				-1, 0, 0,
 				0, 0, 0,
 				0, 0, 1);
+	_visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0);
 
 	//setup menu/actions
 	createMenu();

guilib/src/DataRecorder.cpp

@@ -120,7 +120,7 @@ DataRecorder::~DataRecorder()
 	this->closeRecorder();
 }
 
-void DataRecorder::addData(const rtabmap::SensorData & data)
+void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & pose, const cv::Mat & covariance)
 {
 	memoryMutex_.lock();
 	if(memory_)
@@ -134,10 +134,10 @@ void DataRecorder::addData(const rtabmap::SensorData & data)
 
 		//save to database
 		UTimer time;
-		memory_->update(data);
+		memory_->update(data, pose, covariance);
 		const Signature * s = memory_->getLastWorkingSignature();
-		totalSizeKB_ += (int)s->getImageCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
-		totalSizeKB_ += (int)s->getDepthCompressed().total()/1000;
+		totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
 		memory_->cleanup();
 
 		if(++count_ % 30)
@@ -183,8 +183,8 @@ void DataRecorder::handleEvent(UEvent * event)
 					{
 						processingImages_ = true;
 						QMetaObject::invokeMethod(this, "showImage",
-								Q_ARG(cv::Mat, camEvent->data().image()),
+								Q_ARG(cv::Mat, camEvent->data().imageRaw()),
-								Q_ARG(cv::Mat, camEvent->data().depthOrRightImage()));
+								Q_ARG(cv::Mat, camEvent->data().depthOrRightRaw()));
 					}
 				}
 			}

guilib/src/DatabaseViewer.cpp

@@ -49,7 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/gui/UCv2Qt.h"
 #include "rtabmap/core/util3d.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_surface.h"
@@ -631,23 +630,21 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
-					memory_->addLink(
+					memory_->addLink(Link(
-							refinedIter->second.to(),
 							refinedIter->second.from(),
-							refinedIter->second.transform(),
+							refinedIter->second.to(),
 							refinedIter->second.type(),
-							refinedIter->second.rotVariance(),
+							refinedIter->second.transform(),
-							refinedIter->second.transVariance());
+							refinedIter->second.infMatrix()));
 				}
 				else
 				{
-					memory_->addLink(
+					memory_->addLink(Link(
-							iter->second.to(),
 							iter->second.from(),
-							iter->second.transform(),
+							iter->second.to(),
 							iter->second.type(),
-							iter->second.rotVariance(),
+							iter->second.transform(),
-							iter->second.transVariance());
+							iter->second.infMatrix()));
 				}
 			}
 
@@ -660,8 +657,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 							iter->second.from(),
 							iter->second.to(),
 							iter->second.transform(),
-							iter->second.rotVariance(),
+							iter->second.infMatrix());
-							iter->second.transVariance());
 				}
 			}
 
@@ -760,6 +756,7 @@ void DatabaseViewer::exportDatabase()
 			double previousStamp = 0;
 			std::vector<double> delays(ids_.size());
 			int oi=0;
+			std::map<int, Transform> poses;
 			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
 				Transform odomPose;
@@ -784,6 +781,8 @@ void DatabaseViewer::exportDatabase()
 								delays[oi++] = stamp - previousStamp;
 							}
 							previousStamp = stamp;
+
+							poses.insert(std::make_pair(ids_[i], odomPose));
 						}
 					}
 					if(sessionExported >= 0 && mapId > sessionExported)
@@ -805,31 +804,47 @@ void DatabaseViewer::exportDatabase()
 				{
 					int id = ids.at(i);
 
-					Signature data = memory_->getSignatureData(id, true);
+					SensorData data = memory_->getNodeData(id, true);
-					float rotVariance = 1.0f;
+					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-					float transVariance = 1.0f;
 					if(dialog.isOdomExported())
 					{
-						data.getPoseVariance(rotVariance, transVariance);
+						if(memory_->getSignature(id) == 0)
+						{
+							UERROR("could not find node %d in memory.", id);
+						}
+						else
+						{
+							covariance = memory_->getSignature(id)->getPoseCovariance();
+						}
 					}
-					rtabmap::SensorData sensorData(
-						dialog.isDepth2dExported()?data.getLaserScanRaw():cv::Mat(),
-						dialog.isDepth2dExported()?data.getLaserScanMaxPts():0,
-						dialog.isRgbExported()?data.getImageRaw():cv::Mat(),
-						dialog.isDepthExported()?data.getDepthRaw():cv::Mat(),
-						dialog.isRgbExported() || dialog.isDepthExported()?data.getFx():0,
-						dialog.isRgbExported() || dialog.isDepthExported()?data.getFy():0,
-						dialog.isRgbExported() || dialog.isDepthExported()?data.getCx():0,
-						dialog.isRgbExported() || dialog.isDepthExported()?data.getCy():0,
-						dialog.isRgbExported() || dialog.isDepthExported()?data.getLocalTransform():Transform::getIdentity(),
-						dialog.isOdomExported()?data.getPose():Transform(),
-						rotVariance,
-						transVariance,
-						data.id(),
-						data.getStamp(),
-						dialog.isUserDataExported()?data.getUserData():std::vector<unsigned char>());
 
-					recorder.addData(sensorData);
+					rtabmap::SensorData sensorData;
+					if(data.cameraModels().size())
+					{
+						sensorData = rtabmap::SensorData(
+							dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
+							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							dialog.isRgbExported()?data.imageRaw():cv::Mat(),
+							dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
+							data.cameraModels(),
+							data.id(),
+							data.stamp(),
+							dialog.isUserDataExported()?data.userData():std::vector<unsigned char>());
+					}
+					else
+					{
+						sensorData = rtabmap::SensorData(
+							dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
+							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							dialog.isRgbExported()?data.imageRaw():cv::Mat(),
+							dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
+							data.stereoCameraModel(),
+							data.id(),
+							data.stamp(),
+							dialog.isUserDataExported()?data.userData():std::vector<unsigned char>());
+					}
+
+					recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
 
 					progressDialog->appendText(tr("Exported node %1").arg(id));
 					progressDialog->incrementStep();
@@ -1115,8 +1130,8 @@ void DatabaseViewer::view3DMap()
 	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 	if(ok)
 	{
-		int decimation = item.toInt();
+		int decimation = item.toInt();
-		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
 		if(ok)
 		{
 			std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1154,60 +1169,35 @@ void DatabaseViewer::view3DMap()
 					rtabmap::Transform pose = iter->second;
 					if(!pose.isNull())
 					{
-						Signature data = memory_->getSignatureData(iter->first, true);
+						SensorData data = memory_->getNodeData(iter->first, true);
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-						UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
+						UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
-						UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
+						UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-						if(data.getDepthRaw().type() == CV_8UC1)
+						cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
+
+						if(cloud->size())
 						{
-							cv::Mat leftImg;
+							QColor color = Qt::red;
-							if(data.getImageRaw().channels() == 3)
+							int mapId, weight;
+							Transform odomPose;
+							std::string label;
+							double stamp;
+							std::vector<unsigned char> userData;
+							if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
 							{
-								cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
+								color = (Qt::GlobalColor)(mapId % 12 + 7 );
 							}
-							else
+
-							{
+							viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
-								leftImg = data.getImageRaw();
+
-							}
+							UINFO("Generated %d (%d points)", iter->first, cloud->size());
-							cloud = rtabmap::util3d::cloudFromDisparityRGB(
+							progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
-									data.getImageRaw(),
-								util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
-								data.getCx(), data.getCy(),
-								data.getFx(), data.getFy(),
-								decimation);
 						}
 						else
 						{
-							cloud = rtabmap::util3d::cloudFromDepthRGB(
+							UINFO("Empty cloud %d", iter->first);
-									data.getImageRaw(),
+							progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
-									data.getDepthRaw(),
-									data.getCx(), data.getCy(),
-									data.getFx(), data.getFy(),
-									decimation);
 						}
-
-						if(maxDepth)
-						{
-							cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
-						}
-
-						cloud = rtabmap::util3d::transformPointCloud(cloud, data.getLocalTransform());
-
-						QColor color = Qt::red;
-						int mapId, weight;
-						Transform odomPose;
-						std::string label;
-						double stamp;
-						std::vector<unsigned char> userData;
-						if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
-						{
-							color = (Qt::GlobalColor)(mapId % 12 + 7 );
-						}
-
-						viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
-
-						UINFO("Generated %d (%d points)", iter->first, cloud->size());
-						progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
 						progressDialog.incrementStep();
 						QApplication::processEvents();
 					}
@@ -1239,8 +1229,8 @@ void DatabaseViewer::generate3DMap()
 	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 	if(ok)
 	{
-		int decimation = item.toInt();
+		int decimation = item.toInt();
-		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
 		if(ok)
 		{
 			QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
@@ -1264,48 +1254,24 @@ void DatabaseViewer::generate3DMap()
 						const rtabmap::Transform & pose = iter->second;
 						if(!pose.isNull())
 						{
-							Signature data = memory_->getSignatureData(iter->first, true);
+							SensorData data = memory_->getNodeData(iter->first, true);
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-							UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
+							UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
-							UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
+							UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-							if(data.getDepthRaw().type() == CV_8UC1)
+							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
+							std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
+							if(cloud->size())
 							{
-								cv::Mat leftImg;
+								cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
-								if(data.getImageRaw().channels() == 3)
+								pcl::io::savePCDFile(name, *cloud);
-								{
+								UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
-									cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
+								progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
-								}
-								else
-								{
-									leftImg = data.getImageRaw();
-								}
-								cloud = rtabmap::util3d::cloudFromDisparityRGB(
-										data.getImageRaw(),
-									util2d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
-									data.getCx(), data.getCy(),
-									data.getFx(), data.getFy(),
-									decimation);
 							}
 							else
 							{
-								cloud = rtabmap::util3d::cloudFromDepthRGB(
+								UINFO("Ignored empty cloud %s", name.c_str());
-										data.getImageRaw(),
+								progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
-										data.getDepthRaw(),
-										data.getCx(), data.getCy(),
-										data.getFx(), data.getFy(),
-										decimation);
 							}
-
-							if(maxDepth)
-							{
-								cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
-							}
-
-							cloud = rtabmap::util3d::transformPointCloud(cloud, pose*data.getLocalTransform());
-							std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
-							pcl::io::savePCDFile(name, *cloud);
-							UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
-							progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
 							progressDialog.incrementStep();
 							QApplication::processEvents();
 						}
@@ -1552,19 +1518,21 @@ void DatabaseViewer::update(int value,
 			QImage imgDepth;
 			if(memory_)
 			{
-				Signature data = memory_->getSignatureData(id, true);
+				SensorData data = memory_->getNodeData(id, true);
-				if(!data.getImageRaw().empty())
+				if(!data.imageRaw().empty())
 				{
-					img = uCvMat2QImage(data.getImageRaw());
+					img = uCvMat2QImage(data.imageRaw());
 				}
-				if(!data.getDepthRaw().empty())
+				if(!data.depthOrRightRaw().empty())
 				{
-					imgDepth = uCvMat2QImage(data.getDepthRaw());
+					imgDepth = uCvMat2QImage(data.depthOrRightRaw());
 				}
 
-				if(data.getWords().size())
+				const Signature * signature = memory_->getSignature(id);
+
+				if(signature && signature->getWords().size())
 				{
-					view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow);
+					view->setFeatures(signature->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
 				}
 
 				Transform odomPose;
@@ -1574,16 +1542,16 @@ void DatabaseViewer::update(int value,
 				std::vector<unsigned char> d;
 				memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true);
 
-				weight->setNum(data.getWeight());
+				weight->setNum(w);
-				label->setText(data.getLabel().c_str());
+				label->setText(l.c_str());
 				labelPose->setText(QString("%1%2, %3, %4").arg(odomPose.isIdentity()?"* ":"").arg(odomPose.x()).arg(odomPose.y()).arg(odomPose.z()));
-				if(data.getStamp()!=0.0)
+				if(s!=0.0)
 				{
-					stamp->setText(QDateTime::fromMSecsSinceEpoch(data.getStamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
+					stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
 
 				//stereo
-				if(!data.getDepthRaw().empty() && data.getDepthRaw().type() == CV_8UC1)
+				if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
 				{
 					this->updateStereo(&data);
 				}
@@ -1594,32 +1562,21 @@ void DatabaseViewer::update(int value,
 				}
 
 				// 3d view
-				if(view3D->isVisible() && !data.getDepthRaw().empty())
+				if(view3D->isVisible() && !data.depthOrRightRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					if(data.getDepthRaw().type() == CV_8UC1)
+					cloud = util3d::cloudRGBFromSensorData(data);
+					if(cloud->size())
 					{
-						cloud = util3d::cloudFromStereoImages(
+						view3D->addOrUpdateCloud("0", cloud);
-								data.getImageRaw(),
-								data.getDepthRaw(),
-								data.getCx(), data.getCy(),
-								data.getFx(), data.getFy(),
-								1);
 					}
-					else
-					{
-						cloud = util3d::cloudFromDepthRGB(
-								data.getImageRaw(),
-								data.getDepthRaw(),
-								data.getCx(), data.getCy(),
-								data.getFx(), data.getFy(),
-								1);
-					}
-					view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform());
 
 					//add scan
-					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw());
-					view3D->addOrUpdateCloud("1", scan);
+					if(scan->size())
+					{
+						view3D->addOrUpdateCloud("1", scan);
+					}
 
 					view3D->update();
 				}
@@ -1744,29 +1701,34 @@ void DatabaseViewer::update(int value,
 		view->setSceneRect(rect);
 	}
 }
-
+
 void DatabaseViewer::updateStereo()
 {
 	if(ui_->horizontalSlider_A->maximum())
 	{
 		int id = ids_.at(ui_->horizontalSlider_A->value());
-		Signature data = memory_->getSignatureData(id, true);
+		SensorData data = memory_->getNodeData(id, true);
 		updateStereo(&data);
 	}
 }
 
-void DatabaseViewer::updateStereo(const Signature * data)
+void DatabaseViewer::updateStereo(const SensorData * data)
 {
-	if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1)
+	if(data &&
+		ui_->dockWidget_stereoView->isVisible() &&
+		!data->imageRaw().empty() &&
+		!data->depthOrRightRaw().empty() &&
+		data->depthOrRightRaw().type() == CV_8UC1 &&
+		data->stereoCameraModel().isValid())
 	{
 		cv::Mat leftMono;
-		if(data->getImageRaw().channels() == 3)
+		if(data->imageRaw().channels() == 3)
 		{
-			cv::cvtColor(data->getImageRaw(), leftMono, CV_BGR2GRAY);
+			cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
 		}
 		else
 		{
-			leftMono = data->getImageRaw();
+			leftMono = data->imageRaw();
 		}
 
 		UTimer timer;
@@ -1808,7 +1770,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
 		std::vector<cv::Point2f> rightCorners;
 		cv::calcOpticalFlowPyrLK(
 				leftMono,
-				data->getDepthRaw(),
+				data->depthOrRightRaw(),
 				leftCorners,
 				rightCorners,
 				status,
@@ -1840,13 +1802,16 @@ void DatabaseViewer::updateStereo(const Signature * data)
 						pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 								leftCorners[i],
 								disparity,
-								data->getCx(), data->getCy(), data->getFx(), data->getFy());
+								data->stereoCameraModel().left().cx(),
+								data->stereoCameraModel().left().cy(),
+								data->stereoCameraModel().left().fx(),
+								data->stereoCameraModel().baseline());
 
 						if(pcl::isFinite(tmpPt))
-						{
+						{
-							pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
+							pt = pcl::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform().toEigen3f());
 							status[i] = 100; //blue
-							++inliers;
+							++inliers;
 							cloud->at(oi++) = pt;
 						}
 					}
@@ -1909,8 +1874,8 @@ void DatabaseViewer::updateStereo(const Signature * data)
 		ui_->graphicsView_stereo->setFeaturesShown(false);
 		ui_->graphicsView_stereo->setImageDepthShown(true);
 
-		ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->getImageRaw()));
+		ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
-		ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->getDepthRaw()));
+		ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->depthOrRightRaw()));
 
 		// Draw lines between corresponding features...
 		for(unsigned int i=0; i<kpts.size(); ++i)
@@ -2079,7 +2044,9 @@ void DatabaseViewer::updateConstraintView(
 	UASSERT(!t.isNull() && memory_);
 
 	ui_->label_type->setNum(link.type());
-	ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance())));
+	ui_->label_variance->setText(QString("%1, %2")
+			.arg(sqrt(link.rotVariance()))
+			.arg(sqrt(link.transVariance())));
 	ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
 	if(link.type() == Link::kNeighbor &&
 	   graphes_.size() &&
@@ -2148,15 +2115,15 @@ void DatabaseViewer::updateConstraintView(
 
 	if(ui_->constraintsViewer->isVisible())
 	{
-		Signature dataFrom, dataTo;
+		SensorData dataFrom, dataTo;
 
-		dataFrom = memory_->getSignatureData(link.from(), true);
+		dataFrom = memory_->getNodeData(link.from(), true);
-		UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
+		UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
-		UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
+		UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
 
-		dataTo = memory_->getSignatureData(link.to(), true);
+		dataTo = memory_->getNodeData(link.to(), true);
-		UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
+		UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
-		UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
+		UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
 
 
 		if(cloudFrom->size() == 0 && cloudTo->size() == 0)
@@ -2164,51 +2131,9 @@ void DatabaseViewer::updateConstraintView(
 			//cloud 3d
 			if(!ui_->checkBox_show3DWords->isChecked())
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
-				if(dataFrom.getDepthRaw().type() == CV_8UC1)
+				cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
-				{
+				cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
-					cloudFrom = rtabmap::util3d::cloudFromStereoImages(
-							dataFrom.getImageRaw(),
-							dataFrom.getDepthRaw(),
-							dataFrom.getCx(), dataFrom.getCy(),
-							dataFrom.getFx(), dataFrom.getFy(),
-							1);
-				}
-				else
-				{
-					cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
-							dataFrom.getImageRaw(),
-							dataFrom.getDepthRaw(),
-							dataFrom.getCx(), dataFrom.getCy(),
-							dataFrom.getFx(), dataFrom.getFy(),
-							1);
-				}
-
-				cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
-				cloudFrom = rtabmap::util3d::transformPointCloud(cloudFrom, dataFrom.getLocalTransform());
-
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
-				if(dataTo.getDepthRaw().type() == CV_8UC1)
-				{
-					cloudTo = rtabmap::util3d::cloudFromStereoImages(
-							dataTo.getImageRaw(),
-							dataTo.getDepthRaw(),
-							dataTo.getCx(), dataTo.getCy(),
-							dataTo.getFx(), dataTo.getFy(),
-							1);
-				}
-				else
-				{
-					cloudTo = rtabmap::util3d::cloudFromDepthRGB(
-							dataTo.getImageRaw(),
-							dataTo.getDepthRaw(),
-							dataTo.getCx(), dataTo.getCy(),
-							dataTo.getFx(), dataTo.getFy(),
-							1);
-				}
-
-				cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
-				cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t*dataTo.getLocalTransform());
 
 				if(cloudFrom->size())
 				{
@@ -2216,6 +2141,7 @@ void DatabaseViewer::updateConstraintView(
 				}
 				if(cloudTo->size())
 				{
+					cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
 					ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
 				}
 			}
@@ -2300,8 +2226,8 @@ void DatabaseViewer::updateConstraintView(
 		{
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
+			scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
-			scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
+			scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
 			scanB = rtabmap::util3d::transformPointCloud(scanB, t);
 			if(scanA->size())
 			{
@@ -2413,51 +2339,30 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				bool added = false;
 				if(ui_->groupBox_gridFromProjection->isChecked())
 				{
-					Signature data = memory_->getSignatureData(ids_.at(i), true);
+					SensorData data = memory_->getNodeData(ids_.at(i), true);
-					if(!data.getDepthRaw().empty())
+					if(!data.depthOrRightRaw().empty())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-						if(data.getDepthRaw().type() == CV_8UC1)
+						cloud = util3d::cloudFromSensorData(data,
-						{
+								ui_->spinBox_projDecimation->value(),
-							cloud = rtabmap::util3d::cloudFromDisparity(
+								ui_->doubleSpinBox_projMaxDepth->value(),
-									util2d::disparityFromStereoImages(data.getImageRaw(), data.getDepthRaw()),
+								ui_->doubleSpinBox_gridCellSize->value());
-									data.getCx(),
-									data.getCy(),
-									data.getFx(),
-									data.getFy(),
-									ui_->spinBox_projDecimation->value());
-						}
-						else
-						{
-							cloud = util3d::cloudFromDepth(
-									data.getDepthRaw(),
-									data.getCx(),
-									data.getCy(),
-									data.getFx(),
-									data.getFy(),
-									ui_->spinBox_projDecimation->value());
-						}
-						if(cloud->size())
-						{
-							cloud = util3d::passThrough(cloud, "z", 0, ui_->doubleSpinBox_projMaxDepth->value());
-						}
 
 						if(cloud->size())
 						{
-							cloud = util3d::voxelize(cloud, ui_->doubleSpinBox_gridCellSize->value());
-							cloud = util3d::transformPointCloud(cloud, data.getLocalTransform());
-
 							UTimer timer;
 							float cellSize = ui_->doubleSpinBox_gridCellSize->value();
 							float groundNormalMaxAngle = M_PI_4;
 							int minClusterSize = 20;
 							cv::Mat ground, obstacles;
+
 							util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
 									cloud,
 									ground, obstacles,
 									cellSize,
 									groundNormalMaxAngle,
 									minClusterSize);
+
 							if(!ground.empty() || !obstacles.empty())
 							{
 								localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles)));
@@ -2468,8 +2373,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				}
 				else
 				{
-					Signature data = memory_->getSignatureData(ids_.at(i), false);
+					SensorData data = memory_->getNodeData(ids_.at(i), false);
-					if(!data.getLaserScanCompressed().empty())
+					if(!data.laserScanCompressed().empty())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 						cv::Mat laserScan;
@@ -2804,9 +2709,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	int correspondences = 0;
 	Transform transform;
 
-	Signature dataFrom, dataTo;
+	SensorData dataFrom, dataTo;
-	dataFrom = memory_->getSignatureData(currentLink.from(), false);
+	dataFrom = memory_->getNodeData(currentLink.from(), false);
-	dataTo = memory_->getSignatureData(currentLink.to(), false);
+	dataTo = memory_->getNodeData(currentLink.to(), false);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2816,8 +2721,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
-		cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
+		cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.laserScanCompressed());
-		cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
+		cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.laserScanCompressed());
 
 		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
@@ -2844,9 +2749,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 
 				if(!transform.isNull())
 				{
-					if(dataTo.getLaserScanMaxPts())
+					if(dataTo.laserScanMaxPts())
 					{
-						correspondenceRatio =  float(correspondences)/float(dataTo.getLaserScanMaxPts());
+						correspondenceRatio =  float(correspondences)/float(dataTo.laserScanMaxPts());
 					}
 					else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
 					{
@@ -2859,112 +2764,60 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	else
 	{
 		//3D
-		cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed());
+		cv::Mat im,de;
-		cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed());
+		dataFrom.uncompressData(&im, &de, 0);
-
+		dataTo.uncompressData(&im, &de, 0);
-		if(depthA.type() == CV_8UC1)
+		cloudA = util3d::cloudFromSensorData(dataFrom,
+				ui_->spinBox_icp_decimation->value(),
+				ui_->doubleSpinBox_icp_maxDepth->value(),
+				ui_->doubleSpinBox_icp_voxel->value());
+		cloudB = util3d::cloudFromSensorData(dataTo,
+				ui_->spinBox_icp_decimation->value(),
+				ui_->doubleSpinBox_icp_maxDepth->value(),
+				ui_->doubleSpinBox_icp_voxel->value());
+		if(cloudA->size() && cloudB->size())
 		{
-			cv::Mat leftMono;
+			cloudB = util3d::transformPointCloud(cloudB, t);
-			cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed());
+			if(ui_->checkBox_icp_p2plane->isChecked())
-			if(left.channels() > 1)
 			{
-				cv::cvtColor(left, leftMono, CV_BGR2GRAY);
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
+
+				cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+				if(cloudA->size() != cloudANormals->size())
+				{
+					UWARN("removed nan normals...");
+				}
+
+				cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+				if(cloudB->size() != cloudBNormals->size())
+				{
+					UWARN("removed nan normals...");
+				}
+
+				transform = util3d::icpPointToPlane(cloudBNormals,
+						cloudANormals,
+						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+						ui_->spinBox_icp_iteration->value(),
+						&hasConverged,
+						&variance,
+						&correspondences);
 			}
 			else
 			{
-				leftMono = left;
+				transform = util3d::icp(cloudB,
+						cloudA,
+						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+						ui_->spinBox_icp_iteration->value(),
+						&hasConverged,
+						&variance,
+						&correspondences);
 			}
-			cloudA = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthA), dataFrom.getCx(), dataFrom.getCy(), dataFrom.getFx(), dataFrom.getFy(), ui_->spinBox_icp_decimation->value());
+			correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total());
-			if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
-			{
-				cloudA = util3d::passThrough(cloudA, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
-			}
-			if(ui_->doubleSpinBox_icp_voxel->value() > 0)
-			{
-				cloudA = util3d::voxelize(cloudA, ui_->doubleSpinBox_icp_voxel->value());
-			}
-			cloudA = util3d::transformPointCloud(cloudA, dataFrom.getLocalTransform());
 		}
 		else
 		{
-			cloudA = util3d::getICPReadyCloud(depthA,
+			UWARN("No cloud generated!");
-					dataFrom.getFx(), dataFrom.getFy(), dataFrom.getCx(), dataFrom.getCy(),
-					ui_->spinBox_icp_decimation->value(),
-					ui_->doubleSpinBox_icp_maxDepth->value(),
-					ui_->doubleSpinBox_icp_voxel->value(),
-					0, // no sampling
-					dataFrom.getLocalTransform());
-		}
-		if(depthB.type() == CV_8UC1)
-		{
-			cv::Mat leftMono;
-			cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed());
-			if(left.channels() > 1)
-			{
-				cv::cvtColor(left, leftMono, CV_BGR2GRAY);
-			}
-			else
-			{
-				leftMono = left;
-			}
-			cloudB = util3d::cloudFromDisparity(util2d::disparityFromStereoImages(leftMono, depthB), dataTo.getCx(), dataTo.getCy(), dataTo.getFx(), dataTo.getFy(), ui_->spinBox_icp_decimation->value());
-			if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
-			{
-				cloudB = util3d::passThrough(cloudB, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
-			}
-			if(ui_->doubleSpinBox_icp_voxel->value() > 0)
-			{
-				cloudB = util3d::voxelize(cloudB, ui_->doubleSpinBox_icp_voxel->value());
-			}
-			cloudB = util3d::transformPointCloud(cloudB, t * dataTo.getLocalTransform());
-		}
-		else
-		{
-			cloudB = util3d::getICPReadyCloud(depthB,
-					dataTo.getFx(), dataTo.getFy(), dataTo.getCx(), dataTo.getCy(),
-					ui_->spinBox_icp_decimation->value(),
-					ui_->doubleSpinBox_icp_maxDepth->value(),
-					ui_->doubleSpinBox_icp_voxel->value(),
-					0, // no sampling
-					t * dataTo.getLocalTransform());
-		}
-
-		if(ui_->checkBox_icp_p2plane->isChecked())
-		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
-
-			cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
-			if(cloudA->size() != cloudANormals->size())
-			{
-				UWARN("removed nan normals...");
-			}
-
-			cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
-			if(cloudB->size() != cloudBNormals->size())
-			{
-				UWARN("removed nan normals...");
-			}
-
-			transform = util3d::icpPointToPlane(cloudBNormals,
-					cloudANormals,
-					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-					ui_->spinBox_icp_iteration->value(),
-					&hasConverged,
-					&variance,
-					&correspondences);
-		}
-		else
-		{
-			transform = util3d::icp(cloudB,
-					cloudA,
-					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-					ui_->spinBox_icp_iteration->value(),
-					&hasConverged,
-					&variance,
-					&correspondences);
-
-			correspondenceRatio = float(correspondences)/float(depthB.total());
 		}
 	}
 
@@ -3068,8 +2921,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 		Memory tmpMemory(parameters);
 
 		// Add signatures
-		SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
+		SensorData dataFrom = memory_->getNodeData(from, true);
-		SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
+		SensorData dataTo = memory_->getNodeData(to, true);
 
 		if(from > to)
 		{
@@ -3188,8 +3041,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			Memory tmpMemory(parameters);
 
 			// Add signatures
-			SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
+			SensorData dataFrom = memory_->getNodeData(from, true);
-			SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
+			SensorData dataTo = memory_->getNodeData(to, true);
 
 			if(from > to)
 			{
@@ -3207,8 +3060,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 
 			if(!silent)
 			{
-				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth());
+				ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw());
-				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth());
+				ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw());
 				updateWordsMatching();
 			}
 		}

guilib/src/GraphViewer.cpp

@@ -417,7 +417,8 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 
 	if(wasEmpty)
 	{
-		this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+		QRectF rect = this->scene()->itemsBoundingRect();
+		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
 	}
 }
 

guilib/src/LoopClosureViewer.cpp

@@ -31,7 +31,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -106,74 +105,14 @@ void LoopClosureViewer::updateView(const Transform & transform)
 		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
 		if(!t.isNull())
 		{
-			//cloud 3d
+			//cloud 3d
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
-			if(sA_.getDepthRaw().type() == CV_8UC1)
+			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, 0.0f, samples);
-			{
-				cloudA = util3d::cloudFromStereoImages(
-						sA_.getImageRaw(),
-						sA_.getDepthRaw(),
-						sA_.getCx(), sA_.getCy(),
-						sA_.getFx(), sA_.getFy(),
-						decimation);
-			}
-			else
-			{
-				cloudA = util3d::cloudFromDepthRGB(
-						sA_.getImageRaw(),
-						sA_.getDepthRaw(),
-						sA_.getCx(), sA_.getCy(),
-						sA_.getFx(), sA_.getFy(),
-						decimation);
-			}
-
-			cloudA = util3d::removeNaNFromPointCloud(cloudA);
-
-			if(maxDepth>0.0)
-			{
-				cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
-			}
-			if(samples>0 && (int)cloudA->size() > samples)
-			{
-				cloudA = util3d::sampling(cloudA, samples);
-			}
-			cloudA = util3d::transformPointCloud(cloudA, sA_.getLocalTransform());
-
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
-			if(sB_.getDepthRaw().type() == CV_8UC1)
-			{
-				cloudB = util3d::cloudFromStereoImages(
-						sB_.getImageRaw(),
-						sB_.getDepthRaw(),
-						sB_.getCx(), sB_.getCy(),
-						sB_.getFx(), sB_.getFy(),
-						decimation);
-			}
-			else
-			{
-				cloudB = util3d::cloudFromDepthRGB(
-						sB_.getImageRaw(),
-						sB_.getDepthRaw(),
-						sB_.getCx(), sB_.getCy(),
-						sB_.getFx(), sB_.getFy(),
-						decimation);
-			}
-
-			cloudB = util3d::removeNaNFromPointCloud(cloudB);
-
-			if(maxDepth>0.0)
-			{
-				cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
-			}
-			if(samples>0 && (int)cloudB->size() > samples)
-			{
-				cloudB = util3d::sampling(cloudB, samples);
-			}
 			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, 0.0f, samples);
 
 			//cloud 2d
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw());
+			scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw());
 			scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw());
 			scanB = util3d::transformPointCloud(scanB, t);
 
@@ -184,6 +123,7 @@ void LoopClosureViewer::updateView(const Transform & transform)
 				ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
 			}
 			if(cloudB->size())
+			{
 				cloudB = util3d::transformPointCloud(cloudB, t);
 				ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
 			}

guilib/src/MainWindow.cpp

@@ -86,7 +86,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_mapping.h"
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_registration.h"
@@ -438,9 +437,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
 	connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
 
-	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
-	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
+	connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent)), this, SLOT(processOdometry(rtabmap::OdometryEvent)));
-	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
 
 	connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(notifyNoMoreImages()));
 
@@ -672,12 +670,13 @@ void MainWindow::handleEvent(UEvent* anEvent)
 			if(!_processingOdometry && !_processingStatistics)
 			{
 				_processingOdometry = true; // if we receive too many odometry events!
-				emit odometryReceived(odomEvent->data(), odomEvent->info());
+				emit odometryReceived(*odomEvent);
 			}
 			else
 			{
 				// we receive too many odometry events! just send without data
-				emit odometryReceived(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->info());
+				OdometryEvent tmp(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
+				emit odometryReceived(tmp);
 			}
 		}
 	}
@@ -701,14 +700,14 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 }
 
-void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
+void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 {
 	_processingOdometry = true;
 	UTimer time;
 	// Process Data
-	if(data.isValid())
+	if(!odom.data().imageRaw().empty())
 	{
-		Transform pose = data.pose();
+		Transform pose = odom.pose();
 		bool lost = false;
 		bool lostStateChanged = false;
 
@@ -722,11 +721,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			pose = _lastOdomPose;
 			lost = true;
 		}
-		else if(info.inliers>0 &&
+		else if(odom.info().inliers>0 &&
 				_preferencesDialog->getOdomQualityWarnThr() &&
-				info.inliers < _preferencesDialog->getOdomQualityWarnThr())
+				odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr())
 		{
-			UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
+			UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr());
 			lostStateChanged = _ui->widget_cloudViewer->getBackgroundColor() == Qt::darkRed;
 			_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
 			_ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
@@ -750,84 +749,96 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			if(!pose.isNull())
 			{
 				// 3d cloud
-				if(data.depthOrRightImage().cols == data.image().cols &&
+				if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
-				   data.depthOrRightImage().rows == data.image().rows &&
+				   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
-				   !data.depthOrRightImage().empty() &&
+				   !odom.data().depthOrRightRaw().empty() &&
-				   data.fx() > 0.0f &&
+				   (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) &&
-				   data.fyOrBaseline() > 0.0f &&
 				   _preferencesDialog->isCloudsShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = createCloud(0,
+					cloud = util3d::cloudRGBFromSensorData(odom.data(),
-							data.image(),
-							data.depthOrRightImage(),
-							data.fx(),
-							data.fyOrBaseline(),
-							data.cx(),
-							data.cy(),
-							data.localTransform(),
-							pose,
-							_preferencesDialog->getCloudVoxelSize(1),
 							_preferencesDialog->getCloudDecimation(1),
-							_preferencesDialog->getCloudMaxDepth(1));
+							_preferencesDialog->getCloudMaxDepth(1),
-
+							_preferencesDialog->getCloudVoxelSize(1));
-					if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+					if(cloud->size())
 					{
-						UERROR("Adding cloudOdom to viewer failed!");
+						cloud = util3d::transformPointCloud(cloud, pose);
+
+						if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+						{
+							UERROR("Adding cloudOdom to viewer failed!");
+						}
+						_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
+						_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
+						_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
+					}
+					else
+					{
+						UWARN("Empty cloudOdom!");
+						_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", false);
 					}
-					_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
-					_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
-					_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
 				}
 
 				// 2d cloud
-				if(!data.laserScan().empty() &&
+				if(!odom.data().laserScanRaw().empty() &&
 					_preferencesDialog->isScansShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloud(data.laserScan());
+					cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
 					cloud = util3d::transformPointCloud(cloud, pose);
 					if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
 					{
-						UERROR("Adding scanOdom to viewer failed!");
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+						cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
+						cloud = util3d::transformPointCloud(cloud, pose);
+						if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
+						{
+							UERROR("Adding scanOdom to viewer failed!");
+						}
+						_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
+						_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
+						_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 					}
-					_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
-					_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
-					_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
-				}
-
-				if(!data.pose().isNull())
-				{
-					// update camera position
-					_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*data.pose());
 				}
 			}
-			_ui->widget_cloudViewer->update();
 		}
 
+		if(!odom.pose().isNull())
+		{
+			// update camera position
+			_ui->widget_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
+		}
+		_ui->widget_cloudViewer->update();
+
 		if(_ui->graphicsView_graphView->isVisible())
 		{
-			if(!pose.isNull() && !data.pose().isNull())
+			if(!pose.isNull() && !odom.pose().isNull())
 			{
-				_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
+				_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*odom.pose());
 				_ui->graphicsView_graphView->update();
 			}
 		}
 
 		if(_ui->dockWidget_odometry->isVisible() &&
-		   !data.image().empty())
+		   !odom.data().imageRaw().empty())
 		{
 			if(_ui->imageView_odometry->isFeaturesShown())
 			{
-				if(info.type == 0)
+				if(odom.info().type == 0)
 				{
-					_ui->imageView_odometry->setFeatures(info.words, data.depth(), Qt::yellow);
+					_ui->imageView_odometry->setFeatures(
+							odom.info().words,
+							odom.data().depthRaw(),
+							Qt::yellow);
 				}
-				else if(info.type == 1)
+				else if(odom.info().type == 1)
 				{
 					std::vector<cv::KeyPoint> kpts;
-					cv::KeyPoint::convert(info.refCorners, kpts);
+					cv::KeyPoint::convert(odom.info().refCorners, kpts);
-					_ui->imageView_odometry->setFeatures(kpts, data.depth(), Qt::red);
+					_ui->imageView_odometry->setFeatures(
+							kpts,
+							odom.data().depthRaw(),
+							Qt::red);
 				}
 			}
 
@@ -840,7 +851,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 					_odomImageShow = _ui->imageView_odometry->isImageShown();
 					_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
 				}
-				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
+				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
 				_ui->imageView_odometry->setImageShown(true);
 				_ui->imageView_odometry->setImageDepthShown(true);
 			}
@@ -853,55 +864,55 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 					_ui->imageView_odometry->setImageDepthShown(_odomImageDepthShow);
 				}
 
-				_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
+				_ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
 				if(_ui->imageView_odometry->isImageDepthShown())
 				{
-					_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+					_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
 				}
 
-				if(info.type == 0)
+				if(odom.info().type == 0)
 				{
 					if(_ui->imageView_odometry->isFeaturesShown())
 					{
-						for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+						for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
+							_ui->imageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
 						}
-						for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+						for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
+							_ui->imageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
 						}
 					}
 				}
-			}
+				if(odom.info().type == 1 && odom.info().refCorners.size())
-			if(info.type == 1 && info.cornerInliers.size())
-			{
-				if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
 				{
-					//draw lines
+					if(_ui->imageView_odometry->isFeaturesShown() || _ui->imageView_odometry->isLinesShown())
-					UASSERT(info.refCorners.size() == info.newCorners.size());
-					std::set<int> inliers(info.cornerInliers.begin(), info.cornerInliers.end());
-					for(unsigned int i=0; i<info.refCorners.size(); ++i)
 					{
-						if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
+						//draw lines
+						UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
+						std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
+						for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
 						{
-							_ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
+							if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
-						}
+							{
-						if(_ui->imageView_odometry->isLinesShown())
+								_ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
-						{
+							}
-							_ui->imageView_odometry->addLine(
+							if(_ui->imageView_odometry->isLinesShown())
-									info.refCorners[i].x,
+							{
-									info.refCorners[i].y,
+								_ui->imageView_odometry->addLine(
-									info.newCorners[i].x,
+										odom.info().refCorners[i].x,
-									info.newCorners[i].y,
+										odom.info().refCorners[i].y,
-									inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
+										odom.info().newCorners[i].x,
+										odom.info().newCorners[i].y,
+										inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
+							}
 						}
 					}
 				}
 			}
-			if(!data.image().empty())
+			if(!odom.data().imageRaw().empty())
 			{
-				_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
+				_ui->imageView_odometry->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
 			}
 
 			_ui->imageView_odometry->update();
@@ -914,74 +925,74 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	}
 
 	//Process info
-	if(info.inliers >= 0)
+	if(odom.info().inliers >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
+		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
 	}
-	if(info.matches >= 0)
+	if(odom.info().matches >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
+		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
 	}
-	if(info.variance >= 0)
+	if(odom.info().variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
+		_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)odom.data().id(), sqrt((float)odom.info().variance));
 	}
-	if(info.variance >= 0)
+	if(odom.info().variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
+		_ui->statsToolBox->updateStat("Odometry/Variance/", (float)odom.data().id(), (float)odom.info().variance);
 	}
-	if(info.timeEstimation > 0)
+	if(odom.info().timeEstimation > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)data.id(), (float)info.timeEstimation*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
 	}
-	if(info.timeParticleFiltering > 0)
+	if(odom.info().timeParticleFiltering > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)data.id(), (float)info.timeParticleFiltering*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
 	}
-	if(info.features >=0)
+	if(odom.info().features >=0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
+		_ui->statsToolBox->updateStat("Odometry/Features/", (float)odom.data().id(), (float)odom.info().features);
 	}
-	if(info.localMapSize >=0)
+	if(odom.info().localMapSize >=0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
+		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)odom.data().id(), (float)odom.info().localMapSize);
 	}
-	_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
+	_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());
 
-	float x,y,z, roll,pitch,yaw;
+	float x=0.0f,y,z, roll,pitch,yaw;
-	if(!info.transform.isNull())
+	if(!odom.info().transform.isNull())
 	{
-		info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		_ui->statsToolBox->updateStat("Odometry/Tx/m", (float)data.id(), x);
+		_ui->statsToolBox->updateStat("Odometry/Tx/m", (float)odom.data().id(), x);
-		_ui->statsToolBox->updateStat("Odometry/Ty/m", (float)data.id(), y);
+		_ui->statsToolBox->updateStat("Odometry/Ty/m", (float)odom.data().id(), y);
-		_ui->statsToolBox->updateStat("Odometry/Tz/m", (float)data.id(), z);
+		_ui->statsToolBox->updateStat("Odometry/Tz/m", (float)odom.data().id(), z);
-		_ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)data.id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Troll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Tpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Tyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
 	}
 
-	if(!info.transformFiltered.isNull())
+	if(!odom.info().transformFiltered.isNull())
 	{
-		info.transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-		_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)data.id(), x);
+		_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)odom.data().id(), x);
-		_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)data.id(), y);
+		_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)odom.data().id(), y);
-		_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)data.id(), z);
+		_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)odom.data().id(), z);
-		_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)data.id(), roll*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)data.id(), pitch*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
-		_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)data.id(), yaw*180.0/CV_PI);
+		_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
 	}
 
-	if(info.interval > 0)
+	if(odom.info().interval > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f);
+		_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)odom.data().id(), odom.info().interval*1000.f);
-		_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f);
+		_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)odom.data().id(), x/odom.info().interval*3.6f);
 	}
-	if(info.distanceTravelled > 0)
+	if(odom.info().distanceTravelled > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)data.id(), info.distanceTravelled);
+		_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled);
 	}
 
-	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
+	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0);
 	_processingOdometry = false;
 }
 
@@ -994,8 +1005,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	totalTime.start();
 	//Affichage des stats et images
 
-	int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
+	int refMapId = -1, loopMapId = -1;
-	int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
+	if(uContains(stat.getSignatures(), stat.refImageId()))
+	{
+		refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
+	}
+	if(uContains(stat.getSignatures(), stat.loopClosureId()))
+	{
+		loopMapId = stat.getSignatures().at(stat.loopClosureId()).mapId();
+	}
 
 	_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
 
@@ -1012,12 +1030,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
 
 		// update cache
-		Signature signature = stat.getSignature();
+		Signature signature;
-		signature.uncompressData(); // make sure data are uncompressed
+		if(uContains(stat.getSignatures(), stat.refImageId()))
-		_cachedSignatures.insert(stat.getSignature().id(), signature);
+		{
+			signature = stat.getSignatures().at(stat.refImageId());
+			signature.sensorData().uncompressData(); // make sure data are uncompressed
+			_cachedSignatures.insert(signature.id(), signature);
+		}
 
 		// For intermediate empty nodes, keep latest image shown
-		if(!signature.getImageRaw().empty() || signature.getWords().size())
+		if(!signature.sensorData().imageRaw().empty() || signature.getWords().size())
 		{
 			_ui->imageView_source->clear();
 			_ui->imageView_loopClosure->clear();
@@ -1098,7 +1120,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
 				if(iter != _cachedSignatures.end())
 				{
-					iter.value().uncompressData();
+					iter.value().sensorData().uncompressData();
 					loopSignature = iter.value();
 				}
 			}
@@ -1108,10 +1130,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		//update image views
 		{
-			UCvMat2QImageThread qimageThread(signature.getImageRaw());
+			UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
-			UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
+			UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
-			UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
+			UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
-			UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
+			UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
 			qimageThread.start();
 			qdepthThread.start();
 			qimageLoopThread.start();
@@ -1198,10 +1220,15 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.poses().size())
 		{
 			// update pose only if odometry is not received
+			std::map<int, int> mapIds;
+			for(std::map<int, Signature>::const_iterator iter=stat.getSignatures().begin(); iter!=stat.getSignatures().end();++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
+			}
 			updateMapCloud(stat.poses(),
 					_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
 					stat.constraints(),
-					stat.getMapIds());
+					mapIds);
 
 			_odometryReceived = false;
 
@@ -1213,7 +1240,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			// loop closure view
 			if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0)  &&
 			   !stat.loopClosureTransform().isNull() &&
-			   !loopSignature.getImageRaw().empty())
+			   !loopSignature.sensorData().imageRaw().empty())
 			{
 				// the last loop closure data
 				Transform loopClosureTransform = stat.loopClosureTransform();
@@ -1294,7 +1321,7 @@ void MainWindow::updateMapCloud(
 		{
 			if(!_ui->actionSave_point_cloud->isEnabled() &&
 				_cachedSignatures.size() &&
-				(!(--_cachedSignatures.end())->getDepthCompressed().empty() ||
+				(!(--_cachedSignatures.end())->sensorData().depthOrRightCompressed().empty() ||
 				 !(--_cachedSignatures.end())->getWords3().empty()))
 			{
 				//enable save cloud action
@@ -1304,7 +1331,7 @@ void MainWindow::updateMapCloud(
 
 			if(!_ui->actionView_scans->isEnabled() &&
 				_cachedSignatures.size() &&
-				!(--_cachedSignatures.end())->getLaserScanCompressed().empty())
+				!(--_cachedSignatures.end())->sensorData().laserScanCompressed().empty())
 			{
 				_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1391,7 +1418,7 @@ void MainWindow::updateMapCloud(
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
-					if((!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty()) || jter->getWords3().size())
+					if((!jter->sensorData().imageCompressed().empty() && !jter->sensorData().depthOrRightCompressed().empty()) || jter->getWords3().size())
 					{
 						this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					}
@@ -1427,7 +1454,7 @@ void MainWindow::updateMapCloud(
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
-					if(!jter->getLaserScanCompressed().empty())
+					if(!jter->sensorData().laserScanCompressed().empty())
 					{
 						this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					}
@@ -1605,25 +1632,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		return;
 	}
 
-	if(!iter->getImageCompressed().empty() && !iter->getDepthCompressed().empty())
+	if(!iter->sensorData().imageCompressed().empty() && !iter->sensorData().depthOrRightCompressed().empty())
 	{
 
 		cv::Mat image, depth;
-		iter->uncompressData(&image, &depth, 0);
+		SensorData data = iter->sensorData();
+		data.uncompressData(&image, &depth, 0);
 
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		cloud = createCloud(nodeId,
+		UASSERT(nodeId == data.id());
-				image,
+		cloud = util3d::cloudRGBFromSensorData(data,
-				depth,
-				iter->getFx(),
-				iter->getFy(),
-				iter->getCx(),
-				iter->getCy(),
-				iter->getLocalTransform(),
-				Transform::getIdentity(),
-				_preferencesDialog->getCloudVoxelSize(0),
 				_preferencesDialog->getCloudDecimation(0),
-				_preferencesDialog->getCloudMaxDepth(0));
+				_preferencesDialog->getCloudMaxDepth(0),
+				_preferencesDialog->getCloudVoxelSize(0));
 
 		if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
 		{
@@ -1758,10 +1779,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		return;
 	}
 
-	if(!iter->getLaserScanCompressed().empty())
+	if(!iter->sensorData().laserScanCompressed().empty())
 	{
 		cv::Mat depth2D;
-		iter->uncompressData(0, 0, &depth2D);
+		iter->sensorData().uncompressData(0, 0, &depth2D);
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		cloud = util3d::laserScanToPointCloud(depth2D);
@@ -1977,10 +1998,12 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		QApplication::processEvents();
 
 		int addedSignatures = 0;
+		std::map<int, int> mapIds;
 		for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
 			iter!=event.getSignatures().end();
 			++iter)
 		{
+			mapIds.insert(std::make_pair(iter->first, iter->second.mapId()));
 			if(!_cachedSignatures.contains(iter->first))
 			{
 				_cachedSignatures.insert(iter->first, iter->second);
@@ -2000,7 +2023,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_initProgressDialog->appendText("Updating the 3D map cloud...");
 			_initProgressDialog->incrementStep();
 			QApplication::processEvents();
-			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), event.getMapIds(), true);
+			this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints(), mapIds, true);
 			_initProgressDialog->appendText("Updating the 3D map cloud... done.");
 		}
 		else
@@ -3288,15 +3311,15 @@ void MainWindow::postProcessing()
 			{
 				odomPoses.insert(*iter); // fill raw poses
 			}
-			if(jter->getLocalTransform().isNull())
+			if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid())
 			{
-				UWARN("Local transform of %d is null.", iter->first);
+				UWARN("Calibration of %d is null.", iter->first);
 				allDataAvailable = false;
 			}
 			if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
 			{
 				// depth data required
-				if(jter->getDepthCompressed().empty() || jter->getFx() <= 0.0f || jter->getFy() <= 0.0f)
+				if(jter->sensorData().depthOrRightCompressed().empty())
 				{
 					UWARN("Depth data of %d missing.", iter->first);
 					allDataAvailable = false;
@@ -3305,7 +3328,7 @@ void MainWindow::postProcessing()
 				if(reextractFeatures)
 				{
 					// rgb required
-					if(jter->getImageCompressed().empty())
+					if(jter->sensorData().imageCompressed().empty())
 					{
 						UWARN("Rgb of %d missing.", iter->first);
 						allDataAvailable = false;
@@ -3354,6 +3377,7 @@ void MainWindow::postProcessing()
 	int loopClosuresAdded = 0;
 	if(detectMoreLoopClosures)
 	{
+		UDEBUG("");
 		Memory memory(parameters);
 		if(reextractFeatures)
 		{
@@ -3426,13 +3450,15 @@ void MainWindow::postProcessing()
 							memory.init("", true); // clear previously added signatures
 
 							// Add signatures
-							SensorData dataFrom = signatureFrom.toSensorData();
+							SensorData dataFrom = signatureFrom.sensorData();
-							SensorData dataTo = signatureTo.toSensorData();
+							SensorData dataTo = signatureTo.sensorData();
+
+							cv::Mat image, depth;
+							dataFrom.uncompressData(&image, &depth, 0);
+							dataTo.uncompressData(&image, &depth, 0);
 
 							if(dataFrom.isValid() &&
-							   dataFrom.isMetric() &&
 							   dataTo.isValid() &&
-							   dataTo.isMetric() &&
 							   dataFrom.id() != Memory::kIdInvalid &&
 							   signatureFrom.id() != Memory::kIdInvalid)
 							{
@@ -3502,6 +3528,7 @@ void MainWindow::postProcessing()
 
 	if(refineNeighborLinks || refineLoopClosureLinks)
 	{
+		UDEBUG("");
 		if(refineLoopClosureLinks)
 		{
 			_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
@@ -3556,83 +3583,96 @@ void MainWindow::postProcessing()
 					Signature & signatureTo = _cachedSignatures[to];
 
 					//3D
+					UDEBUG("");
 					cv::Mat depthA, depthB;
-					signatureFrom.uncompressData(0, &depthA, 0);
+					if(signatureFrom.sensorData().stereoCameraModel().isValid())
-					signatureTo.uncompressData(0, &depthB, 0);
-
-					if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
 					{
-						QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
+						cv::Mat leftA, leftB;
-						UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP...");
+						signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0);
-						break;
+						signatureTo.sensorData().uncompressData(&leftB, &depthB, 0);
-					}
-
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
-							signatureFrom.getFx(), signatureFrom.getFy(), signatureFrom.getCx(), signatureFrom.getCy(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples,
-							signatureFrom.getLocalTransform());
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
-							signatureTo.getFx(), signatureTo.getFy(), signatureTo.getCx(), signatureTo.getCy(),
-							decimation,
-							maxDepth,
-							voxelSize,
-							samples,
-							iter->second.transform() * signatureTo.getLocalTransform());
-
-					bool hasConverged = false;
-					double variance = -1;
-					int correspondences = 0;
-					Transform transform;
-					if(pointToPlane)
-					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
-
-						cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
-						if(cloudA->size() != cloudANormals->size())
-						{
-							UWARN("removed nan normals...");
-						}
-
-						cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
-						if(cloudB->size() != cloudBNormals->size())
-						{
-							UWARN("removed nan normals...");
-						}
-
-						transform = util3d::icpPointToPlane(cloudBNormals,
-								cloudANormals,
-								maxCorrespondences,
-								icpIterations,
-								&hasConverged,
-								&variance,
-								&correspondences);
 					}
 					else
 					{
-						transform = util3d::icp(cloudB,
+						signatureFrom.sensorData().uncompressData(0, &depthA, 0);
-								cloudA,
+						signatureTo.sensorData().uncompressData(0, &depthB, 0);
-								maxCorrespondences,
-								icpIterations,
-								&hasConverged,
-								&variance,
-								&correspondences);
 					}
 
-					float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::cloudFromSensorData(
-
+							signatureFrom.sensorData(),
-					if(!transform.isNull() && hasConverged &&
+							decimation,
-					   correspondencesRatio >= correspondenceRatio)
+							maxDepth,
+							voxelSize,
+							samples);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::cloudFromSensorData(
+							signatureTo.sensorData(),
+							decimation,
+							maxDepth,
+							voxelSize,
+							samples);
+					if(cloudA->size() && cloudB->size())
 					{
-						Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
+						cloudB = util3d::transformPointCloud(cloudB, iter->second.transform());
-						iter->second = newLink;
+
+						bool hasConverged = false;
+						double variance = -1;
+						int correspondences = 0;
+						Transform transform;
+						if(pointToPlane)
+						{
+							UDEBUG("");
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
+
+							cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+							if(cloudA->size() != cloudANormals->size())
+							{
+								UWARN("removed nan normals...");
+							}
+
+							cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+							if(cloudB->size() != cloudBNormals->size())
+							{
+								UWARN("removed nan normals...");
+							}
+
+							transform = util3d::icpPointToPlane(cloudBNormals,
+									cloudANormals,
+									maxCorrespondences,
+									icpIterations,
+									&hasConverged,
+									&variance,
+									&correspondences);
+						}
+						else
+						{
+							UDEBUG("");
+							transform = util3d::icp(cloudB,
+									cloudA,
+									maxCorrespondences,
+									icpIterations,
+									&hasConverged,
+									&variance,
+									&correspondences);
+						}
+
+						float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
+
+						if(!transform.isNull() && hasConverged &&
+						   correspondencesRatio >= correspondenceRatio)
+						{
+							Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
+							iter->second = newLink;
+						}
+						else
+						{
+							QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+							_initProgressDialog->appendText(str, Qt::darkYellow);
+							UWARN("%s", str.toStdString().c_str());
+						}
 					}
 					else
 					{
-						QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
+						QString str = tr("Cannot refine link %1->%2 (clouds empty!)").arg(from).arg(to);
 						_initProgressDialog->appendText(str, Qt::darkYellow);
 						UWARN("%s", str.toStdString().c_str());
 					}
@@ -4945,70 +4985,6 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
 	}
 }
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
-		int id,
-		const cv::Mat & rgb,
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
-		const Transform & pose,
-		float voxelSize,
-		int decimation,
-		float maxDepth) const
-{
-	UTimer timer;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-	if(depth.type() == CV_8UC1)
-	{
-		cloud = util3d::cloudFromStereoImages(
-				rgb,
-				depth,
-				cx, cy,
-				fx, fy,
-				decimation);
-	}
-	else
-	{
-		cloud = util3d::cloudFromDepthRGB(
-				rgb,
-				depth,
-				cx, cy,
-				fx, fy,
-				decimation);
-	}
-
-	if(cloud->size())
-	{
-		bool filtered = false;
-		if(cloud->size() && maxDepth)
-		{
-			cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
-			filtered = true;
-		}
-
-		if(cloud->size() && voxelSize)
-		{
-			cloud = util3d::voxelize(cloud, voxelSize);
-			filtered = true;
-		}
-
-		if(cloud->size() && !filtered)
-		{
-			cloud = util3d::removeNaNFromPointCloud(cloud);
-		}
-
-		if(cloud->size())
-		{
-			cloud = util3d::transformPointCloud(cloud, pose * localTransform);
-		}
-	}
-	UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
-	return cloud;
-}
-
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 		const std::map<int, Transform> & poses,
 		float assembledVoxelSize,
@@ -5031,23 +5007,22 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 				if(_cachedSignatures.contains(iter->first))
 				{
 					const Signature & s = _cachedSignatures.find(iter->first).value();
+					SensorData d = s.sensorData();
 					cv::Mat image, depth;
-					s.uncompressDataConst(&image, &depth, 0);
+					d.uncompressData(&image, &depth, 0);
 
 					if(!image.empty() && !depth.empty())
 					{
-						cloud = createCloud(iter->first,
+						UASSERT(iter->first == d.id());
-								image,
+						cloud = util3d::cloudRGBFromSensorData(
-								depth,
+								d,
-								s.getFx(),
-								s.getFy(),
-								s.getCx(),
-								s.getCy(),
-								s.getLocalTransform(),
-								iter->second,
-								regenerateVoxelSize,
 								regenerateDecimation,
-								regenerateMaxDepth);
+								regenerateMaxDepth,
+								regenerateVoxelSize);
+						if(cloud->size())
+						{
+							cloud = util3d::transformPointCloud(cloud, iter->second);
+						}
 					}
 					else if(s.getWords3().size())
 					{
@@ -5135,22 +5110,17 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 				if(_cachedSignatures.contains(iter->first))
 				{
 					const Signature & s = _cachedSignatures.find(iter->first).value();
+					SensorData d = s.sensorData();
 					cv::Mat image, depth;
-					s.uncompressDataConst(&image, &depth, 0);
+					d.uncompressData(&image, &depth, 0);
 					if(!image.empty() && !depth.empty())
 					{
-						cloud =	 createCloud(iter->first,
+						UASSERT(iter->first == d.id());
-							image,
+						cloud =	 util3d::cloudRGBFromSensorData(
-							depth,
+							d,
-							s.getFx(),
-							s.getFy(),
-							s.getCx(),
-							s.getCy(),
-							s.getLocalTransform(),
-							Transform::getIdentity(),
-							regenerateVoxelSize,
 							regenerateDecimation,
-							regenerateMaxDepth);
+							regenerateMaxDepth,
+							regenerateVoxelSize);
 					}
 					else if(s.getWords3().size())
 					{

guilib/src/OdometryViewer.cpp

@@ -61,8 +61,7 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 		validDecimationValue_(1)
 {
 
-	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
-	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
 
 	imageView_->setImageDepthShown(false);
 	imageView_->setMinimumSize(320, 240);
@@ -147,15 +146,15 @@ void OdometryViewer::clear()
 	cloudView_->clear();
 }
 
-void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
+void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 {
 	processingData_ = true;
-	int quality = info.inliers;
+	int quality = odom.info().inliers;
 
 	bool lost = false;
 	bool lostStateChanged = false;
 
-	if(data.pose().isNull())
+	if(odom.pose().isNull())
 	{
 		UDEBUG("odom lost"); // use last pose
 		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
@@ -164,11 +163,11 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 
 		lost = true;
 	}
-	else if(info.inliers>0 &&
+	else if(odom.info().inliers>0 &&
 			qualityWarningThr_ &&
-			info.inliers < qualityWarningThr_)
+			odom.info().inliers < qualityWarningThr_)
 	{
-		UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, qualityWarningThr_);
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
 		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
 		imageView_->setBackgroundColor(Qt::darkYellow);
 		cloudView_->setBackgroundColor(Qt::darkYellow);
@@ -181,16 +180,18 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 		cloudView_->setBackgroundColor(Qt::black);
 	}
 
-	timeLabel_->setText(QString("%1 s").arg(info.timeEstimation));
+	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
 
-	if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f)
+	if(!odom.data().imageRaw().empty() &&
+		!odom.data().depthOrRightRaw().empty() &&
+		(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
 	{
-		UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
+		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
-
+
-		if(!data.depth().empty())
+		if(!odom.data().depthRaw().empty())
 		{
-			if(data.image().cols % decimationSpin_->value() == 0 &&
+			if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
-			   data.image().rows % decimationSpin_->value() == 0)
+			   odom.data().imageRaw().rows % decimationSpin_->value() == 0)
 			{
 				validDecimationValue_ = decimationSpin_->value();
 			}
@@ -199,54 +200,28 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 				UWARN("Decimation (%d) must be a denominator of the width and height of "
 						"the image (%d/%d). Using last valid decimation value (%d).",
 						decimationSpin_->value(),
-						data.image().cols,
+						odom.data().imageRaw().cols,
-						data.image().rows,
+						odom.data().imageRaw().rows,
 						validDecimationValue_);
 			}
 		}
 		else
-		{
+		{
-			validDecimationValue_ = decimationSpin_->value();
+			validDecimationValue_ = decimationSpin_->value();
 		}
 
-
 		// visualization: buffering the clouds
 		// Create the new cloud
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		if(!data.depth().empty())
+		cloud = util3d::cloudRGBFromSensorData(
-		{
+				odom.data(),
-			cloud = util3d::cloudFromDepthRGB(
+				validDecimationValue_,
-					data.image(),
+				0.0f,
-					data.depth(),
+				voxelSpin_->value());
-					data.cx(), data.cy(),
-					data.fx(), data.fy(),
-					validDecimationValue_);
-		}
-		else if(!data.rightImage().empty())
-		{
-			cloud = util3d::cloudFromStereoImages(
-					data.image(),
-					data.rightImage(),
-					data.cx(), data.cy(),
-					data.fx(), data.baseline(),
-					validDecimationValue_);
-		}
-
-		if(maxDepthSpin_->value() > 0.0f && cloud->size())
-		{
-			cloud = util3d::passThrough(cloud, "z", 0, maxDepthSpin_->value());
-		}
-
-		if(voxelSpin_->value() > 0.0f && cloud->size())
-		{
-			cloud = util3d::voxelize(cloud, voxelSpin_->value());
-		}
 
 		if(cloud->size())
 		{
-			cloud = util3d::transformPointCloud(cloud, data.localTransform());
+			if(!odom.pose().isNull())
-
-			if(!data.pose().isNull())
 			{
 				if(cloudView_->getAddedClouds().contains("cloudtmp"))
 				{
@@ -259,10 +234,10 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 					addedClouds_.pop_front();
 				}
 
-				data.id()?id_=data.id():++id_;
+				odom.data().id()?id_=odom.data().id():++id_;
 				std::string cloudName = uFormat("cloud%d", id_);
 				addedClouds_.push_back(cloudName);
-				UASSERT(cloudView_->addCloud(cloudName, cloud, data.pose()));
+				UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
 			}
 			else
 			{
@@ -271,18 +246,18 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 		}
 	}
 
-	if(!data.pose().isNull())
+	if(!odom.pose().isNull())
 	{
-		lastOdomPose_ = data.pose();
+		lastOdomPose_ = odom.pose();
-		cloudView_->updateCameraTargetPosition(data.pose());
+		cloudView_->updateCameraTargetPosition(odom.pose());
 	}
 
-	if(info.localMap.size())
+	if(odom.info().localMap.size())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(info.localMap.size());
+		cloud->resize(odom.info().localMap.size());
 		int i=0;
-		for(std::multimap<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
+		for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
 		{
 			(*cloud)[i].x = iter->second.x;
 			(*cloud)[i].y = iter->second.y;
@@ -291,17 +266,17 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 		cloudView_->addOrUpdateCloud("localmap", cloud);
 	}
 
-	if(!data.image().empty())
+	if(!odom.data().imageRaw().empty())
 	{
-		if(info.type == 0)
+		if(odom.info().type == 0)
 		{
-			imageView_->setFeatures(info.words, data.depth(), Qt::yellow);
+			imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
 		}
-		else if(info.type == 1)
+		else if(odom.info().type == 1)
 		{
 			std::vector<cv::KeyPoint> kpts;
-			cv::KeyPoint::convert(info.refCorners, kpts);
+			cv::KeyPoint::convert(odom.info().refCorners, kpts);
-			imageView_->setFeatures(kpts, data.depth(), Qt::red);
+			imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
 		}
 
 		imageView_->clearLines();
@@ -313,7 +288,7 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 				odomImageShow_ = imageView_->isImageShown();
 				odomImageDepthShow_ = imageView_->isImageDepthShown();
 			}
-			imageView_->setImageDepth(uCvMat2QImage(data.image()));
+			imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
 			imageView_->setImageShown(true);
 			imageView_->setImageDepthShown(true);
 		}
@@ -326,55 +301,55 @@ void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap
 				imageView_->setImageDepthShown(odomImageDepthShow_);
 			}
 
-			imageView_->setImage(uCvMat2QImage(data.image()));
+			imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
 			if(imageView_->isImageDepthShown())
 			{
-				imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+				imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
 			}
 
-			if(info.type == 0)
+			if(odom.info().type == 0)
 			{
 				if(imageView_->isFeaturesShown())
 				{
-					for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+					for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
 					{
-						imageView_->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
+						imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
 					}
-					for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+					for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
 					{
-						imageView_->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
+						imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
 					}
 				}
 			}
 		}
-		if(info.type == 1 && info.cornerInliers.size())
+		if(odom.info().type == 1 && odom.info().cornerInliers.size())
 		{
 			if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
 			{
 				//draw lines
-				UASSERT(info.refCorners.size() == info.newCorners.size());
+				UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
-				for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
+				for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
 				{
 					if(imageView_->isFeaturesShown())
 					{
-						imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
+						imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
 					}
 					if(imageView_->isLinesShown())
 					{
 						imageView_->addLine(
-								info.refCorners[info.cornerInliers[i]].x,
+								odom.info().refCorners[odom.info().cornerInliers[i]].x,
-								info.refCorners[info.cornerInliers[i]].y,
+								odom.info().refCorners[odom.info().cornerInliers[i]].y,
-								info.newCorners[info.cornerInliers[i]].x,
+								odom.info().newCorners[odom.info().cornerInliers[i]].x,
-								info.newCorners[info.cornerInliers[i]].y,
+								odom.info().newCorners[odom.info().cornerInliers[i]].y,
 								Qt::blue);
 					}
 				}
 			}
 		}
 
-		if(!data.image().empty())
+		if(!odom.data().imageRaw().empty())
 		{
-			imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
+			imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
 		}
 	}
 
@@ -395,8 +370,7 @@ void OdometryViewer::handleEvent(UEvent * event)
 			{
 				processingData_ = true;
 				QMetaObject::invokeMethod(this, "processData",
-						Q_ARG(rtabmap::SensorData, odomEvent->data()),
+						Q_ARG(rtabmap::OdometryEvent, *odomEvent));
-						Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
 			}
 		}
 	}

guilib/src/PdfPlot.cpp

@@ -70,10 +70,10 @@ void PdfPlotItem::showDescription(bool shown)
 		{
 			QImage img;
 			QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
-			if(iter != _signaturesRef->constEnd() && !iter.value().getImageCompressed().empty())
+			if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty())
 			{
 				cv::Mat image;
-				iter.value().uncompressDataConst(&image, 0, 0);
+				iter.value().sensorData().uncompressDataConst(&image, 0, 0);
 				if(!image.empty())
 				{
 					img = uCvMat2QImage(image);

tools/Camera/main.cpp

@@ -189,7 +189,7 @@ int main(int argc, char * argv[])
 	}
 
 	cv::Mat rgb;
-	rgb = camera?camera->takeImage():dbReader->getNextData().image();
+	rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
 	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
 	while(!rgb.empty())
 	{
@@ -199,7 +199,7 @@ int main(int argc, char * argv[])
 		if(c == 27)
 			break; // if ESC, break and quit
 
-		rgb = camera?camera->takeImage():dbReader->getNextData().image();
+		rgb = camera?camera->takeImage():dbReader->getNextData().data().imageRaw();
 	}
 	cv::destroyWindow("Video");
 	if(camera)

tools/CameraRGBD/main.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util3d.h"
-#include "rtabmap/core/util3d_conversions.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UMath.h"

utilite/include/rtabmap/utilite/UMath.h

@@ -63,6 +63,28 @@ inline bool uIsFinite(const T & value)
 #endif
 }
 
+/**
+ * Get the minimum of the 3 values.
+ * @return the minimum value
+ */
+template<class T>
+inline T uMin3( const T& a, const T& b, const T& c)
+{
+	float m=a<b?a:b;
+	return m<c?m:c;
+}
+
+/**
+ * Get the maximum of the 3 values.
+ * @return the maximum value
+ */
+template<class T>
+inline T uMax3( const T& a, const T& b, const T& c)
+{
+	float m=a>b?a:b;
+	return m>c?m:c;
+}
+
 /**
  * Get the maximum of a vector.
  * @param v the array