Updated version to 0.8.0

Libraries are installed in lib directly with symbolic links, not in lib/rtabmap-0.8. Removed the need of RPATH in cmake. Saving variance of each link in database (new field Link.variance). The variance is used to generate the constraint information matrices for TORO optimization. ICP: computing variance instead of fitness. ICP3: added correspondences ratio parameter Added OdometryInfo class Refactoring: renamed depth2d stuff to laserScan. rtabmap::Memory and rtabmap::Signature classes (no more distinct neighbor, loop closure or child loop closure links, only links with different types)
2026-09-02 01:20:25 +08:00 · 2014-12-14 16:42:10 -05:00
parent 6acf374063
commit 744e2fb3c7
42 changed files with 1764 additions and 1460 deletions
--- a/corelib/include/rtabmap/core/CameraEvent.h
+++ b/corelib/include/rtabmap/core/CameraEvent.h
@@ -49,18 +49,21 @@ public:
 		data_(image, seq)
 	{
 	}
+
 	CameraEvent() :
 		UEvent(kCodeNoMoreImages)
 	{
 	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
+
+	CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
 		UEvent(kCodeImageDepth),
-		data_(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
+		data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
 	{
 	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
+
+	CameraEvent(const SensorData & data) :
 		UEvent(kCodeImageDepth),
-		data_(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
+		data_(data)
 	{
 	}

--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -40,11 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"

 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>

 namespace rtabmap {

 class Signature;
-class SMSignature;
 class VWDictionary;
 class VisualWord;

@@ -96,11 +96,10 @@ public:

 	// Specific queries...
 	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
+	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
 	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
 	void getPose(int signatureId, Transform & pose, int & mapId) const;
-	void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
-	void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
+	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
 	void getLastNodeId(int & id) const;
@@ -131,11 +130,10 @@ private:
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
-	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const = 0;
-	virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
+	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 & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) 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) const = 0;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
--- a/corelib/include/rtabmap/core/DBReader.h
+++ b/corelib/include/rtabmap/core/DBReader.h
@@ -34,6 +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 <opencv2/core/core.hpp>

@@ -53,14 +54,7 @@ public:

 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	void getNextImage(cv::Mat & image,
-			cv::Mat & depth,
-			cv::Mat & depth2d,
-			float & fx, float & fy,
-			float & cx, float & cy,
-			Transform & localTransform,
-			Transform & pose,
-			int & seq);
+	SensorData getNextData();

 protected:
 	virtual void mainLoopBegin();
--- a/corelib/include/rtabmap/core/Link.h
+++ b/corelib/include/rtabmap/core/Link.h
@@ -39,14 +39,16 @@ public:
 	Link() :
 		from_(0),
 		to_(0),
-		type_(kUndef)
+		type_(kUndef),
+		variance_(1.0f)
 	{
 	}
-	Link(int from, int to, const Transform & transform, Type type) :
+	Link(int from, int to, Type type, const Transform & transform, float variance) :
 		from_(from),
 		to_(to),
 		transform_(transform),
-		type_(type)
+		type_(type),
+		variance_(variance)
 	{
 	}

@@ -56,12 +58,20 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
+	float variance() const {return variance_;}
+
+	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 variance) {variance_ = variance;}

 private:
 	int from_;
 	int to_;
 	Transform transform_;
 	Type type_;
+	float variance_;
 };

 }
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -80,8 +80,8 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform);
+	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
+	void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
 	std::map<int, int> getNeighborsId(int signatureId,
 			int margin,
 			int maxCheckedInDatabase = -1,
@@ -98,12 +98,9 @@ public:
 	void getPose(int locationId,
 			Transform & pose,
 			bool lookInDatabase = false) const;
-	std::map<int, Transform> getNeighborLinks(int signatureId,
-			bool ignoreNeighborByLoopClosure = false,
+	std::map<int, Link> getNeighborLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	void getLoopClosureIds(int signatureId,
-			std::map<int, Transform> & loopClosureIds,
-			std::map<int, Transform> & childLoopClosureIds,
+	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
 	bool isRawDataKept() const {return _rawDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
@@ -154,19 +151,21 @@ public:
 	int getBowMinInliers() const {return _bowMinInliers;}
 	float getBowMaxDepth() const {return _bowMaxDepth;}
 	bool getBowForce2D() const {return _bowForce2D;}
-	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0) const;
-	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0) const;
+	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
 			const std::map<int, Transform> & poses,
-			std::string * rejectedMsg = 0);
+			std::string * rejectedMsg = 0,
+			int * inliers = 0,
+			double * variance = 0);

 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature);
+	void addSignatureToStm(Signature * signature, float odomVariance);
 	void clear();
 	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);

@@ -244,12 +243,11 @@ private:
 	int _icpSamples;
 	float _icpMaxCorrespondenceDistance;
 	int _icpMaxIterations;
-	float _icpMaxFitness;
+	float _icpCorrespondenceRatio;
 	bool _icpPointToPlane;
 	int _icpPointToPlaneNormalNeighbors;
 	float _icp2MaxCorrespondenceDistance;
 	int _icp2MaxIterations;
-	float _icp2MaxFitness;
 	float _icp2CorrespondenceRatio;
 	float _icp2VoxelSize;

--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>

 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/OdometryInfo.h>

 #include <opencv2/opencv.hpp>

@@ -56,7 +57,7 @@ class RTABMAP_EXP Odometry
 {
 public:
 	virtual ~Odometry() {}
-	Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	Transform process(const SensorData & data, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 	bool isLargeEnoughTransform(const Transform & transform);
@@ -75,7 +76,7 @@ public:
 	float getAngularUpdate() const {return _angularUpdate;}

 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;

 private:
 	int _maxFeatures;
@@ -110,7 +111,7 @@ public:
 	const Memory * getMemory() const {return _memory;}

 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);

 private:
 	//Parameters
@@ -133,10 +134,10 @@ public:
 	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}

 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
-	Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
-	Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
-	Transform computeTransformMono(const SensorData & image, int * quality, int * features);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
+	Transform computeTransformStereo(const SensorData & image, OdometryInfo * info);
+	Transform computeTransformRGBD(const SensorData & image, OdometryInfo * info);
+	Transform computeTransformMono(const SensorData & image, OdometryInfo * info);
 private:
 	//Parameters:
 	int flowWinSize_;
@@ -170,13 +171,13 @@ public:
 			int samples = 0,
 			float maxCorrespondenceDistance = 0.05f,
 			int maxIterations = 30,
-			float maxFitness = 0.01f,
+			float correspondenceRatio = 0.7f,
 			bool pointToPlane = true,
 			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());

 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);

 private:
 	int _decimation;
@@ -184,7 +185,7 @@ private:
 	float _samples;
 	float _maxCorrespondenceDistance;
 	int	_maxIterations;
-	float _maxFitness;
+	float _correspondenceRatio;
 	bool _pointToPlane;

 	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
--- a/corelib/include/rtabmap/core/OdometryEvent.h
+++ b/corelib/include/rtabmap/core/OdometryEvent.h
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap/utilite/UEvent.h"
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/OdometryInfo.h"

 namespace rtabmap {

@@ -37,29 +38,20 @@ class OdometryEvent : public UEvent
 {
 public:
 	OdometryEvent(
-		const SensorData & data, int quality = -1, float time = 0.0f, int features = 0, int localMapSize = 0) :
+		const SensorData & data, const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
-			_quality(quality),
-			_time(time),
-			_features(features),
-			_localMapSize(localMapSize)
+			_info(info)
 	{}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}

 	bool isValid() const {return !_data.pose().isNull();}
 	const SensorData & data() const {return _data;}
-	int quality() const {return _quality;}
-	float time() const {return _time;} // seconds
-	int features() const {return _features;}
-	int localMapSize() const {return _localMapSize;}
+	const OdometryInfo & info() const {return _info;}

 private:
 	SensorData _data;
-	int _quality;
-	float _time; // seconds
-	int _features;
-	int _localMapSize;
+	OdometryInfo _info;
 };

 class OdometryResetEvent : public UEvent
--- a/corelib/include/rtabmap/core/OdometryInfo.h
+++ b/corelib/include/rtabmap/core/OdometryInfo.h
@@ -0,0 +1,56 @@
+/*
+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 ODOMETRYINFO_H_
+#define ODOMETRYINFO_H_
+
+namespace rtabmap {
+
+class OdometryInfo
+{
+public:
+	OdometryInfo() :
+		lost(true),
+		matches(-1),
+		inliers(-1),
+		variance(-1),
+		features(-1),
+		localMapSize(-1),
+		time(0.0f)
+	{}
+	bool lost;
+	int matches;
+	int inliers;
+	float variance;
+	int features;
+	int localMapSize;
+	float time;
+};
+
+}
+
+#endif /* ODOMETRYINFO_H_ */
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -283,6 +283,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
+	RTABMAP_PARAM(RGBD, ToroIgnoreVariance,   bool, false,    "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");

 	// Local loop closure detection
@@ -344,13 +345,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
-	RTABMAP_PARAM(LccIcp3, MaxFitness,      float, 1.0, 	"ICP 3D: Maximum fitness to accept the computed transform.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"ICP 3D: Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"ICP 3D: Use point to plane ICP.");
 	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"ICP 3D: Number of neighbors to compute normals for point to plane.");

 	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
 	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
-	RTABMAP_PARAM(LccIcp2, MaxFitness,      float, 1.0, 			"ICP 2D: Maximum fitness to accept the computed transform.");
 	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");

--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -157,6 +157,7 @@ private:
 	float _localDetectMaxNeighbors;
 	int _localDetectMaxDiffID;
 	int _toroIterations;
+	bool _toroIgnoreVariance;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	bool _reextractLoopClosureFeatures;
--- a/corelib/include/rtabmap/core/SensorData.h
+++ b/corelib/include/rtabmap/core/SensorData.h
@@ -52,20 +52,22 @@ public:
 		  float fyOrBaseline,
 		  float cx,
 		  float cy,
-		  const Transform & pose,
 		  const Transform & localTransform,
+		  const Transform & pose,
+		  float poseVariance,
 		  int id = 0);

-	// Metric constructor + 2d depth
-	SensorData(const cv::Mat & image,
+	// Metric constructor + 2d laser scan
+	SensorData(const cv::Mat & laserScan,
+		  const cv::Mat & image,
 		  const cv::Mat & depthOrRightImage,
-		  const cv::Mat & depth2d,
 		  float fx,
 		  float fyOrBaseline,
 		  float cx,
 		  float cy,
-		  const Transform & pose,
 		  const Transform & localTransform,
+		  const Transform & pose,
+		  float poseVariance,
 		  int id = 0);

 	virtual ~SensorData() {}
@@ -80,11 +82,11 @@ public:
 	void setId(int id) {_id = id;}

 	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
-	void setPose(const Transform & pose) {_pose = pose;}
+	void setPose(const Transform & pose, float variance) {_pose = pose; _poseVariance=variance;}
 	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 & depth2d() const {return _depth2d;}
+	const cv::Mat & laserScan() const {return _laserScan;}
 	float fx() const {return _fx;}
 	float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
 	float cx() const {return _cx;}
@@ -93,6 +95,7 @@ public:
 	float fyOrBaseline() const {return _fyOrBaseline;}
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}
+	float poseVariance() const {return _poseVariance;}

 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
@@ -108,13 +111,14 @@ private:

 	// Metric stuff
 	cv::Mat _depthOrRightImage;
-	cv::Mat _depth2d;
+	cv::Mat _laserScan;
 	float _fx;
 	float _fyOrBaseline;
 	float _cx;
 	float _cy;
 	Transform _pose;
 	Transform _localTransform;
+	float _poseVariance;

 	// features
 	std::vector<cv::KeyPoint> _keypoints;
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Link.h>

 namespace rtabmap
 {
@@ -56,7 +57,7 @@ public:
 			const std::multimap<int, cv::KeyPoint> & words,
 			const std::multimap<int, pcl::PointXYZ> & words3,
 			const Transform & pose = Transform(),
-			const cv::Mat & depth2D = cv::Mat(),
+			const cv::Mat & laserScan = cv::Mat(),
 			const cv::Mat & image = cv::Mat(),
 			const cv::Mat & depth = cv::Mat(),
 			float fx = 0.0f,
@@ -75,34 +76,27 @@ public:
 	int id() const {return _id;}
 	int mapId() const {return _mapId;}

-	void addNeighbors(const std::map<int, Transform> & neighbors);
-	void addNeighbor(int neighbor, const Transform & transform = Transform());
-	void removeNeighbor(int neighborId);
-	void removeNeighbors();
-	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
 	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
+	int getWeight() const {return _weight;}

-	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
-	void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
-	void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
-	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
-	void changeLoopClosureId(int idFrom, int idTo);
+	void addLinks(const std::list<Link> & links);
+	void addLinks(const std::map<int, Link> & links);
+	void addLink(const Link & link);

-	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
-	void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
-	void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
+	bool hasLink(int idTo) const;
+
+	void changeLinkIds(int idFrom, int idTo);
+
+	void removeLinks();
+	void removeLink(int idTo);

 	void setSaved(bool saved) {_saved = saved;}
-	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
-	void changeNeighborIds(int idFrom, int idTo);
+	void setModified(bool modified) {_modified = modified; _linksModified = modified;}

-	const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
-	int getWeight() const {return _weight;}
-	const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
-	const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
+	const std::map<int, Link> & getLinks() const {return _links;}
 	bool isSaved() const {return _saved;}
-	bool isModified() const {return _modified || _neighborsModified;}
-	bool isNeighborsModified() const {return _neighborsModified;}
+	bool isModified() const {return _modified || _linksModified;}
+	bool isLinksModified() const {return _linksModified;}

 	//visual words stuff
 	void removeAllWords();
@@ -121,12 +115,12 @@ public:
 	//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 setDepth2DCompressed(const cv::Mat & bytes) {_depth2DCompressed = bytes;}
+	void setLaserScanCompressed(const cv::Mat & bytes) {_laserScanCompressed = bytes;}
 	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 & getDepth2DCompressed() const {return _depth2DCompressed;}
+	const cv::Mat & getLaserScanCompressed() const {return _laserScanCompressed;}
 	float getDepthFx() const {return _fx;}
 	float getDepthFy() const {return _fy;}
 	float getDepthCx() const {return _cx;}
@@ -135,24 +129,22 @@ public:
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
 	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setDepth2DRaw(const cv::Mat & depth2D) {_depth2DRaw = depth2D;}
-	const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
+	void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
+	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}

 	SensorData toSensorData();
 	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw);
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const;
+	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw);
+	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const;

 private:
 	int _id;
 	int _mapId;
-	std::map<int, Transform> _neighbors; // id, transform
+	std::map<int, Link> _links; // id, transform
 	int _weight;
-	std::map<int, Transform> _loopClosureIds; // id, transform
-	std::map<int, Transform> _childLoopClosureIds; // id, transform
 	bool _saved; // If it's saved to bd
 	bool _modified;
-	bool _neighborsModified; // Optimization when updating signatures in database
+	bool _linksModified; // Optimization when updating signatures in database

 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
@@ -163,7 +155,7 @@ private:
 	cv::Mat _imageCompressed; // compressed image

 	cv::Mat _depthCompressed; // compressed image
-	cv::Mat _depth2DCompressed; // compressed data
+	cv::Mat _laserScanCompressed; // compressed data
 	float _fx;
 	float _fy;
 	float _cx;
@@ -174,7 +166,7 @@ private:

 	cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
 	cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _depth2DRaw; // CV_32FC2
+	cv::Mat _laserScanRaw; // CV_32FC2
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -232,8 +232,8 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,
 		int type = CV_32FC1);

-cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);

 std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
 cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
@@ -298,31 +298,35 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		bool refineModel = false,
 		double refineModelSigma = 3.0,
 		int refineModelIterations = 10,
-		std::vector<int> * inliers = 0);
+		std::vector<int> * inliers = 0,
+		double * variance = 0);

 Transform RTABMAP_EXP icp(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
+		bool * hasConverged = 0,
+		double * variance = 0,
+		int * inliers = 0);

 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
+		bool * hasConverged = 0,
+		double * variance = 0,
+		int * inliers = 0);

 Transform RTABMAP_EXP icp2D(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
+		bool * hasConverged = 0,
+		double * variance = 0,
+		int * inliers = 0);

 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -463,6 +467,7 @@ void RTABMAP_EXP optimizeTOROGraph(
 		std::map<int, Transform> & optimizedPoses,
 		int toroIterations = 100,
 		bool toroInitialGuess = true,
+		bool ignoreCovariance = false,
 		std::list<std::map<int, Transform> > * intermediateGraphes = 0);

 void RTABMAP_EXP optimizeTOROGraph(
@@ -471,6 +476,7 @@ void RTABMAP_EXP optimizeTOROGraph(
 		std::map<int, Transform> & optimizedPoses,
 		int toroIterations = 100,
 		bool toroInitialGuess = true,
+		bool ignoreCovariance = false,
 		std::list<std::map<int, Transform> > * intermediateGraphes = 0);

 bool RTABMAP_EXP saveTOROGraph(