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)

CMakeLists.txt

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

bin/.gitignore

@@ -19,3 +19,9 @@
 /librtabmap_cored.so
 /librtabmap_guid.so
 /librtabmap_utilited.so
+/librtabmap_core.so.0.8
+/librtabmap_core.so.0.8.0
+/librtabmap_gui.so.0.8
+/librtabmap_gui.so.0.8.0
+/librtabmap_utilite.so.0.8
+/librtabmap_utilite.so.0.8.0

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)
 	{
 	}
 

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 loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
-	void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) 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 loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
-	virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) 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;

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,
+	SensorData getNextData();
-			cv::Mat & depth,
-			cv::Mat & depth2d,
-			float & fx, float & fy,
-			float & cx, float & cy,
-			Transform & localTransform,
-			Transform & pose,
-			int & seq);
 
 protected:
 	virtual void mainLoopBegin();

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_;
 };
 
 }

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);
+	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform);
+	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,
+	std::map<int, Link> getNeighborLinks(int signatureId,
-			bool ignoreNeighborByLoopClosure = false,
 			bool lookInDatabase = false) const;
-	void getLoopClosureIds(int signatureId,
+	std::map<int, Link> getLoopClosureLinks(int signatureId,
-			std::map<int, Transform> & loopClosureIds,
-			std::map<int, Transform> & childLoopClosureIds,
 			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(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) 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);
+	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) const;
+	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;
 

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);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
-	Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
+	Transform computeTransformStereo(const SensorData & image, OdometryInfo * info);
-	Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
+	Transform computeTransformRGBD(const SensorData & image, OdometryInfo * info);
-	Transform computeTransformMono(const SensorData & image, int * quality, int * features);
+	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

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),
+			_info(info)
-			_time(time),
-			_features(features),
-			_localMapSize(localMapSize)
 	{}
 	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;}
+	const OdometryInfo & info() const {return _info;}
-	float time() const {return _time;} // seconds
-	int features() const {return _features;}
-	int localMapSize() const {return _localMapSize;}
 
 private:
 	SensorData _data;
-	int _quality;
+	OdometryInfo _info;
-	float _time; // seconds
-	int _features;
-	int _localMapSize;
 };
 
 class OdometryResetEvent : public UEvent

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_ */

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.");
 

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;

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
+	// Metric constructor + 2d laser scan
-	SensorData(const cv::Mat & image,
+	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;

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 addLinks(const std::list<Link> & links);
-	void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
+	void addLinks(const std::map<int, Link> & links);
-	void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
+	void addLink(const Link & link);
-	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
-	void changeLoopClosureId(int idFrom, int idTo);
 
-	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
+	bool hasLink(int idTo) const;
-	void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
+
-	void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
+	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 setModified(bool modified) {_modified = modified; _linksModified = modified;}
-	void changeNeighborIds(int idFrom, int idTo);
 
-	const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
+	const std::map<int, Link> & getLinks() const {return _links;}
-	int getWeight() const {return _weight;}
-	const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
-	const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
 	bool isSaved() const {return _saved;}
-	bool isModified() const {return _modified || _neighborsModified;}
+	bool isModified() const {return _modified || _linksModified;}
-	bool isNeighborsModified() const {return _neighborsModified;}
+	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;}
+	void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
-	const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
+	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
 
 	SensorData toSensorData();
 	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw);
+	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw);
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const;
+	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

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);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
+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,
+		bool * hasConverged = 0,
-		double & fitnessScore);
+		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,
+		bool * hasConverged = 0,
-		double & fitnessScore);
+		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,
+		bool * hasConverged = 0,
-		double & fitnessScore);
+		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(

corelib/src/DBDriver.cpp

@@ -406,7 +406,7 @@ void DBDriver::getNodeData(
 		int signatureId,
 		cv::Mat & imageCompressed,
 		cv::Mat & depthCompressed,
-		cv::Mat & depth2dCompressed,
+		cv::Mat & laserScanCompressed,
 		float & fx,
 		float & fy,
 		float & cx,
@@ -414,7 +414,7 @@ void DBDriver::getNodeData(
 		Transform & localTransform) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, depth2dCompressed, fx, fy, cx, cy, localTransform);
+	this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -435,10 +435,10 @@ void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
 }
 
 //TODO Check also in the trash ?
-void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
+void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadNeighborsQuery(signatureId, neighbors);
+	this->loadLinksQuery(signatureId, links, type);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -450,14 +450,6 @@ void DBDriver::getWeight(int signatureId, int & weight) const
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
-void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
-{
-	_dbSafeAccessMutex.lock();
-	this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
-	_dbSafeAccessMutex.unlock();
-}
-
 //TODO Check also in the trash ?
 void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 {

corelib/src/DBDriverSqlite3.cpp

@@ -555,11 +555,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					//Create the depth2d
+					//Create the laserScan
-					cv::Mat depth2dCompressed;
 					if(dataSize>4 && data)
 					{
-						(*iter)->setDepth2DCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
+						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
 					}
 
 				}
@@ -584,7 +583,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		int signatureId,
 		cv::Mat & imageCompressed,
 		cv::Mat & depthCompressed,
-		cv::Mat & depth2dCompressed,
+		cv::Mat & laserScanCompressed,
 		float & fx,
 		float & fy,
 		float & cx,
@@ -681,7 +680,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 			//Create the depth2d
 			if(dataSize>4 && data)
 			{
-				depth2dCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+				laserScanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 			}
 
 			if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
@@ -820,7 +819,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 				  << "FROM Node "
 				  << "LEFT OUTER JOIN Link "
 				  << "ON id = from_id "
-				  << "WHERE type!=1 "
+				  << "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
 				  << "ORDER BY id";
 		}
 
@@ -840,7 +839,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 		// 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=%f", timer.ticks());
+		ULOGGER_DEBUG("Time=%f ids=%d", timer.ticks(), (int)ids.size());
 	}
 }
 
@@ -962,72 +961,6 @@ void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
 	}
 }
 
-void DBDriverSqlite3::loadLoopClosuresQuery(int nodeId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
-{
-	loopIds.clear();
-	childIds.clear();
-	if(_ppDb)
-	{
-		int rc = SQLITE_OK;
-		sqlite3_stmt * ppStmt = 0;
-		std::stringstream query;
-
-		query << "SELECT to_id, type, transform FROM Link WHERE from_id =  "
-			  << nodeId
-			  << " AND type > 0"
-			  << ";";
-
-		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());
-
-		int toId = 0;
-		int type;
-		const void * data = 0;
-		int dataSize = 0;
-
-		// Process the result if one
-		rc = sqlite3_step(ppStmt);
-		while(rc == SQLITE_ROW)
-		{
-			int index = 0;
-			toId = sqlite3_column_int(ppStmt, index++);
-			type = sqlite3_column_int(ppStmt, index++);
-
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-
-			Transform transform;
-			if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
-			{
-				memcpy(transform.data(), data, dataSize);
-			}
-
-			if(nodeId == toId)
-			{
-				UERROR("Loop links cannot be auto-reference links (node=%d)", toId);
-			}
-			else if(type == 1)
-			{
-				UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 1);
-				//loop id
-				loopIds.insert(std::pair<int, Transform>(toId, transform));
-			}
-			else if(type == 2)
-			{
-				UDEBUG("Load link from %d to %d, type=%d", nodeId, toId, 2);
-				//loop id
-				childIds.insert(std::pair<int, Transform>(toId, transform));
-			}
-			rc = sqlite3_step(ppStmt);
-		}
-		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());
-	}
-}
-
 //may be slower than the previous version but don't have a limit of words that can be loaded at the same time
 void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & nodes) const
 {
@@ -1413,7 +1346,10 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 	}
 }
 
-void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const
+void DBDriverSqlite3::loadLinksQuery(
+		int signatureId,
+		std::map<int, Link> & neighbors,
+		Link::Type typeIn) const
 {
 	neighbors.clear();
 	if(_ppDb)
@@ -1424,15 +1360,38 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT to_id, transform FROM Link "
+		if(uStrNumCmp(_version, "0.7.4") >= 0)
-		      << "WHERE from_id = " << signatureId
+		{
-		      << " AND type = 0"
+			query << "SELECT to_id, type, transform, variance FROM Link ";
-			  << " ORDER BY to_id";
+		}
+		else
+		{
+			query << "SELECT to_id, type, transform FROM Link ";
+		}
+		query << "WHERE from_id = " << signatureId;
+		if(typeIn != Link::kUndef)
+		{
+			if(uStrNumCmp(_version, "0.7.4") >= 0)
+			{
+				query << " AND type = " << typeIn;
+			}
+			else if(typeIn == Link::kNeighbor)
+			{
+				query << " AND type = 0";
+			}
+			else if(typeIn > Link::kNeighbor)
+			{
+				query << " AND type > 0";
+			}
+		}
+		query << " ORDER BY to_id";
 
 		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());
 
 		int toId = -1;
+		int type = Link::kUndef;
+		float variance = 1.0f;
 		const void * data = 0;
 		int dataSize = 0;
 
@@ -1443,6 +1402,7 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
 			int index = 0;
 
 			toId = sqlite3_column_int(ppStmt, index++);
+			type = sqlite3_column_int(ppStmt, index++);
 
 			data = sqlite3_column_blob(ppStmt, index);
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -1453,7 +1413,17 @@ void DBDriverSqlite3::loadNeighborsQuery(int signatureId, std::map<int, Transfor
 				memcpy(transform.data(), data, dataSize);
 			}
 
-			neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
+			if(uStrNumCmp(_version, "0.7.4") >= 0)
+			{
+				variance = sqlite3_column_double(ppStmt, index++);
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
+			}
+			else
+			{
+				// neighbor is 0, loop closures are 1 and 2 (child)
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, variance)));
+			}
+
 			rc = sqlite3_step(ppStmt);
 		}
 
@@ -1481,9 +1451,18 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;
 
-		query << "SELECT to_id, type, transform FROM Link "
+		if(uStrNumCmp(_version, "0.7.4") >= 0)
-			  << "WHERE from_id = ? "
+		{
-			  << "ORDER BY to_id";
+			query << "SELECT to_id, type, variance, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else
+		{
+			query << "SELECT to_id, type, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
 
 		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());
@@ -1496,9 +1475,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 			int toId = -1;
 			int linkType = -1;
-			std::map<int, Transform> neighbors;
+			float variance = 1.0f;
-			std::map<int, Transform> loopIds;
+			std::list<Link> links;
-			std::map<int, Transform> childIds;
 			const void * data = 0;
 			int dataSize = 0;
 
@@ -1510,34 +1488,33 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
+				if(uStrNumCmp(_version, "0.7.4") >= 0)
+				{
+					variance = sqlite3_column_double(ppStmt, index++);
+				}
 
 				//transform
 				data = sqlite3_column_blob(ppStmt, index);
 				dataSize = sqlite3_column_bytes(ppStmt, index++);
 				Transform transform;
-				if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
+				UASSERT((unsigned int)dataSize == transform.size()*sizeof(float) && data);
+				memcpy(transform.data(), data, dataSize);
+
+				if(linkType >= 0 && linkType != Link::kUndef)
 				{
-					memcpy(transform.data(), data, dataSize);
+					if(uStrNumCmp(_version, "0.7.4") >= 0)
+					{
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
+					}
+					else // neighbor is 0, loop closures are 1 and 2 (child)
+					{
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, variance));
+					}
 				}
 				else
 				{
-					UFATAL("");
+					UFATAL("Not supported link type %d ! (fromId=%d, toId=%d)",
-				}
+							linkType, (*iter)->id(), toId);
-
-				if(linkType == 1)
-				{
-					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 1);
-					loopIds.insert(std::pair<int, Transform>(toId, transform));
-				}
-				else if(linkType == 2)
-				{
-					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 2);
-					childIds.insert(std::pair<int, Transform>(toId, transform));
-				}
-				else if(linkType == 0)
-				{
-					UDEBUG("Load link from %d to %d, type=%d", (*iter)->id(), toId, 0);
-					neighbors.insert(neighbors.end(), std::pair<int, Transform>(toId, transform));
 				}
 
 				++totalLinksLoaded;
@@ -1546,14 +1523,12 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 			// add links
-			(*iter)->addNeighbors(neighbors);
+			(*iter)->addLinks(links);
-			(*iter)->setLoopClosureIds(loopIds);
-			(*iter)->setChildLoopClosureIds(childIds);
 
 			//reset
 			rc = sqlite3_reset(ppStmt);
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-			UDEBUG("time=%fs, node=%d, neighbors.size=%d, loopIds=%d, childIds=%d", timer.ticks(), (*iter)->id(), neighbors.size(), loopIds.size(), childIds.size());
+			UDEBUG("time=%fs, node=%d, links.size=%d", timer.ticks(), (*iter)->id(), links.size());
 		}
 
 		// Finalize (delete) the statement
@@ -1610,7 +1585,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
 		{
-			if((*j)->isNeighborsModified())
+			if((*j)->isLinksModified())
 			{
 				rc = sqlite3_bind_int(ppStmt, 1, (*j)->id());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1632,24 +1607,13 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		for(std::list<Signature *>::const_iterator j=nodes.begin(); j!=nodes.end(); ++j)
 		{
-			if((*j)->isNeighborsModified())
+			if((*j)->isLinksModified())
 			{
-				// Save neighbor links
+				// Save links
-				const std::map<int, Transform> & neighbors = (*j)->getNeighbors();
+				const std::map<int, Link> & links = (*j)->getLinks();
-				for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+				for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 				{
-					stepLink(ppStmt, (*j)->id(), i->first, 0, i->second);
+					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
-				}
-				// save loop closure links
-				const std::map<int, Transform> & loopIds = (*j)->getLoopClosureIds();
-				for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
-				{
-					stepLink(ppStmt, (*j)->id(), i->first, 1, i->second);
-				}
-				const std::map<int, Transform> & childIds = (*j)->getChildLoopClosureIds();
-				for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
-				{
-					stepLink(ppStmt, (*j)->id(), i->first, 2, i->second);
 				}
 			}
 		}
@@ -1752,22 +1716,11 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 		for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
 		{
-			// Save neighbor links
+			// Save links
-			const std::map<int, Transform> & neighbors = (*jter)->getNeighbors();
+			const std::map<int, Link> & links = (*jter)->getLinks();
-			for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+			for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 			{
-				stepLink(ppStmt, (*jter)->id(), i->first, 0, i->second);
+				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
-			}
-			// save loop closure links
-			const std::map<int, Transform> & loopIds = (*jter)->getLoopClosureIds();
-			for(std::map<int, Transform>::const_iterator i=loopIds.begin(); i!=loopIds.end(); ++i)
-			{
-				stepLink(ppStmt, (*jter)->id(), i->first, 1, i->second);
-			}
-			const std::map<int, Transform> & childIds = (*jter)->getChildLoopClosureIds();
-			for(std::map<int, Transform>::const_iterator i=childIds.begin(); i!=childIds.end(); ++i)
-			{
-				stepLink(ppStmt, (*jter)->id(), i->first, 2, i->second);
 			}
 		}
 		// Finalize (delete) the statement
@@ -1833,9 +1786,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
 			//metric
-			if(!(*i)->getDepthCompressed().empty() || !(*i)->getDepth2DCompressed().empty())
+			if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getDepth2DCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
+				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
 			}
 		}
 		// Finalize (delete) the statement
@@ -2055,9 +2008,16 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepLink() const
 {
-	return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
+	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	{
+		return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
+	}
+	else
+	{
+		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
+	}
 }
-void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const
+void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
 {
 	if(!ppStmt)
 	{
@@ -2072,6 +2032,13 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	rc = sqlite3_bind_int(ppStmt, index++, type);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, variance);
+		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);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 

corelib/src/DBDriverSqlite3.h

@@ -68,18 +68,14 @@ private:
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
-	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
+	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
-	virtual void loadLoopClosuresQuery(
-			int signatureId,
-			std::map<int, Transform> & loopIds,
-			std::map<int, Transform> & childIds) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
 	virtual void getNodeDataQuery(
 			int signatureId,
 			cv::Mat & imageCompressed,
 			cv::Mat & depthCompressed,
-			cv::Mat & depth2dCompressed,
+			cv::Mat & laserScanCompressed,
 			float & fx,
 			float & fy,
 			float & cx,
@@ -113,7 +109,7 @@ private:
 			float cx,
 			float cy,
 			const Transform & localTransform) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, 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

@@ -131,36 +131,24 @@ void DBReader::mainLoopBegin()
 
 void DBReader::mainLoop()
 {
-	cv::Mat image, depth, depth2d;
+	SensorData data = this->getNextData();
-	float fx,fy,cx,cy;
+	if(data.isValid())
-	Transform localTransform, pose;
-	int seq = 0;
-	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
-	if(!image.empty())
 	{
-		if(depth.empty())
+		if(!_odometryIgnored)
 		{
-			this->post(new CameraEvent(image));
+			if(data.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));
 		}
 		else
 		{
-			if(!_odometryIgnored)
+			this->post(new CameraEvent(data));
-			{
-				SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
-				this->post(new OdometryEvent(data));
-				if(pose.isNull())
-				{
-					UWARN("Reading the database: odometry is null! "
-						  "Please set \"Ignore odometry = true\" if there is "
-						  "no odometry in the database.");
-				}
-			}
-			else
-			{
-				// without odometry
-				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
-			}
 		}
+
 	}
 	else if(!this->isKilled())
 	{
@@ -171,18 +159,9 @@ void DBReader::mainLoop()
 
 }
 
-void DBReader::getNextImage(
+SensorData DBReader::getNextData()
-		cv::Mat & image,
-		cv::Mat & depth,
-		cv::Mat & depth2d,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		Transform & pose,
-		int & seq)
 {
+	SensorData data;
 	if(_dbDriver)
 	{
 		float frameRate = _frameRate;
@@ -209,11 +188,24 @@ void DBReader::getNextImage(
 		{
 			cv::Mat imageBytes;
 			cv::Mat depthBytes;
-			cv::Mat depth2dBytes;
+			cv::Mat laserScanBytes;
 			int mapId;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
+			float fx,fy,cx,cy;
-			_dbDriver->getPose(*_currentId, pose, mapId);
+			Transform localTransform, pose;
-			seq = *_currentId;
+			float variance = 1.0f;
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
+			if(!_odometryIgnored)
+			{
+				_dbDriver->getPose(*_currentId, pose, mapId);
+				std::map<int, Link> links;
+				_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
+				if(links.size())
+				{
+					// assume the first is the backward neighbor, take its variance
+					variance = links.begin()->second.variance();
+				}
+			}
+			int seq = *_currentId;
 			++_currentId;
 			if(imageBytes.empty())
 			{
@@ -222,22 +214,29 @@ void DBReader::getNextImage(
 
 			util3d::CompressionThread ctImage(imageBytes, true);
 			util3d::CompressionThread ctDepth(depthBytes, true);
-			util3d::CompressionThread ctDepth2D(depth2dBytes, false);
+			util3d::CompressionThread ctLaserScan(laserScanBytes, false);
 			ctImage.start();
 			ctDepth.start();
-			ctDepth2D.start();
+			ctLaserScan.start();
 			ctImage.join();
 			ctDepth.join();
-			ctDepth2D.join();
+			ctLaserScan.join();
-			image = ctImage.getUncompressedData();
+			data = SensorData(
-			depth = ctDepth.getUncompressedData();
+					ctLaserScan.getUncompressedData(),
-			depth2d = ctDepth2D.getUncompressedData();
+					ctImage.getUncompressedData(),
+					ctDepth.getUncompressedData(),
+					fx,fy,cx,cy,
+					localTransform,
+					pose,
+					variance,
+					seq);
 		}
 	}
 	else
 	{
 		UERROR("Not initialized...");
 	}
+	return data;
 }
 
 } /* namespace rtabmap */

corelib/src/Features2d.cpp

@@ -324,7 +324,7 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 	if(RTABMAP_NONFREE == 0 &&
 	   (type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
 	{
-		UERROR("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
+		UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
 		type = Feature2D::kFeatureOrb;
 	}
 	Feature2D * feature2D = 0;

corelib/src/Odometry.cpp

@@ -118,14 +118,22 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
 			fabs(yaw) > _angularUpdate;
 }
 
-Transform Odometry::process(SensorData & data, int * quality, int * features, int * localMapSize)
+Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 {
+	UTimer time;
 	if(_pose.isNull())
 	{
 		_pose.setIdentity(); // initialized
 	}
 
-	Transform t = this->computeTransform(data, quality, features, localMapSize);
+	Transform t = this->computeTransform(data, info);
+
+	if(info)
+	{
+		info->time = time.elapsed();
+		info->lost = t.isNull();
+	}
+
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -134,14 +142,10 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
 		{
 			float x,y,z, roll,pitch,yaw;
 			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-			_pose *= Transform(x,y,0,0,0,yaw);
+			t = Transform(x,y,0, 0,0,yaw);
-		}
-		else
-		{
-			_pose *= t;
 		}
 
-		return _pose;
+		return _pose *= t;
 	}
 	else if(_resetCurrentCount > 0)
 	{
@@ -231,11 +235,14 @@ void OdometryBOW::reset(const Transform & initialPose)
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryBOW::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
+Transform OdometryBOW::computeTransform(
+		const SensorData & data,
+		OdometryInfo * info)
 {
 	UTimer timer;
 	Transform output;
 
+	double variance = -1;
 	int inliers = 0;
 	int correspondences = 0;
 	int nFeatures = 0;
@@ -276,10 +283,9 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 
 				UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
 
+				correspondences = (int)inliers1->size();
 				if((int)inliers1->size() >= this->getMinInliers())
 				{
-					correspondences = (int)inliers1->size();
-
 					// transform new words in local map referential
 					//inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, this->getPose());
 
@@ -291,7 +297,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 							this->getInlierDistance(),
 							this->getIterations(),
 							this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-							&inliersV);
+							&inliersV,
+							&variance);
 
 					inliers = (int)inliersV.size();
 					if(!transform.isNull())
@@ -362,11 +369,6 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 					transform = transform * icpT;
 					*/
 
-					if(quality)
-					{
-						*quality = inliers;
-					}
-
 					if(inliers < this->getMinInliers())
 					{
 						transform.setNull();
@@ -469,13 +471,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 		_memory->emptyTrash();
 	}
 
-	if(features)
+	if(info)
 	{
-		*features = nFeatures;
+		info->variance = variance;
-	}
+		info->inliers = inliers;
-	if(localMapSize)
+		info->matches = correspondences;
-	{
+		info->features = nFeatures;
-		*localMapSize = (int)localMap_.size();
+		info->localMapSize = (int)localMap_.size();
 	}
 
 	UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
@@ -547,32 +549,30 @@ void OdometryOpticalFlow::reset(const Transform & initialPose)
 // return not null transform if odometry is correctly computed
 Transform OdometryOpticalFlow::computeTransform(
 		const SensorData & data,
-		int * quality,
+		OdometryInfo * info)
-		int * features,
-		int * localMapSize)
 {
 	UDEBUG("");
 
 	if(!data.rightImage().empty())
 	{
 		//stereo
-		return computeTransformStereo(data, quality, features);
+		return computeTransformStereo(data, info);
 	}
 	else
 	{
 		//rgbd
-		return computeTransformRGBD(data, quality, features);
+		return computeTransformRGBD(data, info);
 	}
 }
 
 Transform OdometryOpticalFlow::computeTransformStereo(
 		const SensorData & data,
-		int * quality,
+		OdometryInfo * info)
-		int * features)
 {
 	UTimer timer;
 	Transform output;
 
+	double variance = -1;
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -747,15 +747,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 						this->getInlierDistance(),
 						this->getIterations(),
 						this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-						&inliersV);
+						&inliersV,
+						&variance);
 				UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
 
 				inliers = (int)inliersV.size();
-				if(quality)
-				{
-					*quality = inliers;
-				}
-
 				if(inliers < this->getMinInliers())
 				{
 					output.setNull();
@@ -841,6 +837,14 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		output.setNull();
 	}
 
+	if(info)
+	{
+		info->variance = variance;
+		info->inliers = inliers;
+		info->features = (int)newCorners.size();
+		info->matches = correspondences;
+	}
+
 	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
 			timer.elapsed(),
 			inliers,
@@ -853,12 +857,12 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 
 Transform OdometryOpticalFlow::computeTransformRGBD(
 		const SensorData & data,
-		int * quality,
+		OdometryInfo * info)
-		int * features)
 {
 	UTimer timer;
 	Transform output;
 
+	double variance = -1;
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -967,15 +971,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 					this->getInlierDistance(),
 					this->getIterations(),
 					this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-					&inliersV);
+					&inliersV,
+					&variance);
 			UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
 
 			inliers = (int)inliersV.size();
-			if(quality)
-			{
-				*quality = inliers;
-			}
-
 			if(inliers < this->getMinInliers())
 			{
 				output.setNull();
@@ -1097,6 +1097,14 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 		output = Transform::getIdentity();
 	}
 
+	if(info)
+	{
+		info->variance = variance;
+		info->inliers = inliers;
+		info->features = (int)newCorners.size();
+		info->matches = correspondences;
+	}
+
 	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
 			timer.elapsed(),
 			inliers,
@@ -1112,7 +1120,7 @@ OdometryICP::OdometryICP(int decimation,
 		int samples,
 		float maxCorrespondenceDistance,
 		int maxIterations,
-		float maxFitness,
+		float correspondenceRatio,
 		bool pointToPlane,
 		const ParametersMap & odometryParameter) :
 	Odometry(odometryParameter),
@@ -1121,7 +1129,7 @@ OdometryICP::OdometryICP(int decimation,
 	_samples(samples),
 	_maxCorrespondenceDistance(maxCorrespondenceDistance),
 	_maxIterations(maxIterations),
-	_maxFitness(maxFitness),
+	_correspondenceRatio(correspondenceRatio),
 	_pointToPlane(pointToPlane),
 	_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
 	_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
@@ -1136,13 +1144,13 @@ void OdometryICP::reset(const Transform & initialPose)
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryICP::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
+Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
 {
 	UTimer timer;
 	Transform output;
 
 	bool hasConverged = false;
-	double fitness = 0;
+	double variance = -1;
 	unsigned int minPoints = 100;
 	if(!data.depth().empty())
 	{
@@ -1177,27 +1185,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 
 			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
 			{
+				int correspondences = 0;
 				Transform transform = util3d::icpPointToPlane(newCloud,
 						_previousCloudNormal,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						hasConverged,
+						&hasConverged,
-						fitness);
+						&variance,
+						&correspondences);
 
-				//pcl::io::savePCDFile("old.pcd", *_previousCloud);
+				// verify if there are enough correspondences
-				//pcl::io::savePCDFile("new.pcd", *newCloud);
+				float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
-				//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
-				//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
 
-				if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
+				if(!transform.isNull() && hasConverged &&
+		   	   	   correspondencesRatio >= _correspondenceRatio)
 				{
 					output = transform;
 					_previousCloudNormal = newCloud;
 				}
 				else
 				{
-					UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
+					UWARN("Transform not valid (hasConverged=%s variance = %f)",
-							hasConverged?"true":"false", fitness, _maxFitness);
+							hasConverged?"true":"false", variance);
 				}
 			}
 			else if(newCloud->size() > minPoints)
@@ -1211,27 +1220,28 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 			//point to point
 			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
 			{
+				int correspondences = 0;
 				Transform transform = util3d::icp(newCloudXYZ,
 						_previousCloud,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						hasConverged,
+						&hasConverged,
-						fitness);
+						&variance,
+						&correspondences);
 
-				//pcl::io::savePCDFile("old.pcd", *_previousCloudNormal);
+				// verify if there are enough correspondences
-				//pcl::io::savePCDFile("new.pcd", *newCloud);
+				float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
-				//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
-				//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
 
-				if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
+				if(!transform.isNull() && hasConverged &&
+				   correspondencesRatio >= _correspondenceRatio)
 				{
 					output = transform;
 					_previousCloud = newCloudXYZ;
 				}
 				else
 				{
-					UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
+					UWARN("Transform not valid (hasConverged=%s variance = %f)",
-							hasConverged?"true":"false", fitness, _maxFitness);
+							hasConverged?"true":"false", variance);
 				}
 			}
 			else if(newCloudXYZ->size() > minPoints)
@@ -1246,10 +1256,15 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 		UERROR("Depth is empty?!?");
 	}
 
-	UINFO("Odom update time = %fs hasConverged=%s fitness=%f cloud=%d",
+	if(info)
+	{
+		info->variance = variance;
+	}
+
+	UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
 			timer.elapsed(),
 			hasConverged?"true":"false",
-			fitness,
+			variance,
 			(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
 
 	return output;
@@ -1316,13 +1331,10 @@ void OdometryThread::mainLoop()
 	getData(data);
 	if(data.isValid())
 	{
-		int quality = -1;
+		OdometryInfo info;
-		int features = -1;
+		Transform pose = _odometry->process(data, &info);
-		int localMapSize = -1;
+		data.setPose(pose, info.variance); // a null pose notify that odometry could not be computed
-		UTimer time;
+		this->post(new OdometryEvent(data, info));
-		Transform pose = _odometry->process(data, &quality, &features, &localMapSize);
-		data.setPose(pose); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(data, quality, time.elapsed(), features, localMapSize));
 	}
 }
 

corelib/src/Rtabmap.cpp

@@ -98,6 +98,7 @@ Rtabmap::Rtabmap() :
 	_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
 	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
 	_toroIterations(Parameters::defaultRGBDToroIterations()),
+	_toroIgnoreVariance(Parameters::defaultRGBDToroIgnoreVariance()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -358,6 +359,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
+	Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), _toroIgnoreVariance);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -831,11 +833,11 @@ bool Rtabmap::process(const SensorData & data)
 			//============================================================
 			// Minimum displacement required to add to Memory
 			//============================================================
-			const std::map<int, Transform> & neighbors = signature->getNeighbors();
+			const std::map<int, Link> & links = signature->getLinks();
-			if(neighbors.size() == 1)
+			if(links.size() == 1)
 			{
 				float x,y,z, roll,pitch,yaw;
-				neighbors.begin()->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 				if(fabs(x) < _rgbdLinearUpdate &&
 					fabs(y) < _rgbdLinearUpdate &&
 					fabs(z) < _rgbdLinearUpdate &&
@@ -857,26 +859,27 @@ bool Rtabmap::process(const SensorData & data)
 		// Scan matching
 		//============================================================
 		if(_poseScanMatching &&
-			signature->getNeighbors().size() == 1 &&
+			signature->getLinks().size() == 1 &&
-			!signature->getDepth2DCompressed().empty() &&
+			!signature->getLaserScanCompressed().empty() &&
 			rehearsedId == 0) // don't do it if rehearsal happened
 		{
 			UINFO("Odometry correction by scan matching");
-			int oldId = signature->getNeighbors().begin()->first;
+			int oldId = signature->getLinks().begin()->first;
 			const Signature * oldS = _memory->getSignature(oldId);
 			UASSERT(oldS != 0);
 			std::string rejectedMsg;
-			Transform guess = signature->getNeighbors().begin()->second;
+			Transform guess = signature->getLinks().begin()->second.transform();
-			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg);
+			double variance = -1.0;
+			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, 0, &variance);
 			if(!t.isNull())
 			{
 				scanMatchingSuccess = true;
 				UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
 						signature->id(),
 						oldId,
-						signature->getNeighbors().at(oldId).prettyPrint().c_str(),
+						signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 						t.prettyPrint().c_str());
-				_memory->updateNeighborLink(signature->id(), oldId, t);
+				_memory->updateNeighborLink(signature->id(), oldId, t, variance);
 			}
 			else
 			{
@@ -886,9 +889,9 @@ bool Rtabmap::process(const SensorData & data)
 		timeScanMatching = timer.ticks();
 		ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
 
-		if(signature->getNeighbors().size() == 1)
+		if(signature->getLinks().size() == 1)
 		{
-			_constraints.insert(std::make_pair(signature->id(), Link(signature->id(), signature->getNeighbors().begin()->first, signature->getNeighbors().begin()->second, Link::kNeighbor)));
+			_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
 		}
 
 		//============================================================
@@ -902,15 +905,17 @@ bool Rtabmap::process(const SensorData & data)
 			for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
 			{
 				if(*iter != signature->id() &&
-				   signature->getNeighbors().find(*iter) == signature->getNeighbors().end() &&
+				   signature->getLinks().find(*iter) == signature->getLinks().end() &&
 				   _memory->getSignature(*iter)->mapId() == signature->mapId())
 				{
 					std::string rejectedMsg;
 					UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
-					Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg);
+					double variance = -1.0;
+					int inliers = -1;
+					Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
 					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 					{
-						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg);
+						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
 						float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
 						if(!icpTransform.isNull() &&
 							_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -932,7 +937,7 @@ bool Rtabmap::process(const SensorData & data)
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLoopClosureLink(*iter, signature->id(), transform, false))
+						if(_memory->addLoopClosureLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1251,6 +1256,7 @@ bool Rtabmap::process(const SensorData & data)
 	{
 		//Compute transform if metric data are present
 		Transform transform;
+		double variance = -1;
 		if(_rgbdSlamMode)
 		{
 			std::string rejectedMsg;
@@ -1298,7 +1304,7 @@ bool Rtabmap::process(const SensorData & data)
 					memory.update(dataFrom);
 					UDEBUG("timeUpFrom = %fs", timeT.ticks());
 
-					transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers);
+					transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
 					UDEBUG("timeTransform = %fs", timeT.ticks());
 				}
 				else
@@ -1306,16 +1312,16 @@ bool Rtabmap::process(const SensorData & data)
 					// Fallback to normal way (raw data not kept in database...)
 					UWARN("Loop closure: Some images not found in memory for re-extracting "
 						  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
-					transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
+					transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
 				}
 			}
 			else
 			{
-				transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers);
+				transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
 			}
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg);
+				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
 				float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
 				if(!icpTransform.isNull() &&
 					_globalLoopClosureIcpMaxDistance>0.0f &&
@@ -1339,7 +1345,7 @@ bool Rtabmap::process(const SensorData & data)
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, true);
+			rejectedHypothesis = !_memory->addLoopClosureLink(_lcHypothesisId, signature->id(), transform, Link::kGlobalClosure, variance);
 		}
 
 		if(rejectedHypothesis)
@@ -1362,7 +1368,7 @@ bool Rtabmap::process(const SensorData & data)
 	int localSpaceNearestId = 0;
 	if(_lcHypothesisId == 0 &&
 	   _localLoopClosureDetectionSpace &&
-	   !signature->getDepth2DCompressed().empty())
+	   !signature->getLaserScanCompressed().empty())
 	{
 		if(_toroIterations == 0)
 		{
@@ -1386,10 +1392,11 @@ bool Rtabmap::process(const SensorData & data)
 			//The nearest will be the reference for a loop closure transform
 			if(poses.size() &&
 				localSpaceNearestId &&
-				signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
+				signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
 			{
+				double variance = 1.0;
 				std::string rejectedMsg;
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
+				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg, 0, &variance);
 				if(!t.isNull())
 				{
 					localSpaceClosureId = localSpaceNearestId;
@@ -1397,7 +1404,7 @@ bool Rtabmap::process(const SensorData & data)
 							signature->id(),
 							localSpaceNearestId,
 							t.prettyPrint().c_str());
-					_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
+					_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
 
 					// Old map -> new map, used for localization correction on loop closure
 					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1531,9 +1538,9 @@ bool Rtabmap::process(const SensorData & data)
 			}
 			if(_lcHypothesisId || localSpaceClosureId)
 			{
-				UASSERT(uContains(sLoop->getLoopClosureIds(), signature->id()));
+				UASSERT(uContains(sLoop->getLinks(), signature->id()));
-				UINFO("Set loop closure transform = %s", sLoop->getLoopClosureIds().at(signature->id()).prettyPrint().c_str());
+				UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
-				statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
+				statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
 			}
 
 			if(!_rgbdSlamMode)
@@ -1602,10 +1609,9 @@ bool Rtabmap::process(const SensorData & data)
 	// global loop closure detection before starting the new map,
 	// otherwise it deletes the current node.
 	if(_startNewMapOnLoopClosure &&
-		_memory->isIncremental() &&                        // only in mapping mode
+		_memory->isIncremental() &&              // only in mapping mode
-		signature->getChildLoopClosureIds().size() == 0 && // no loop closure
+		signature->getLinks().size() == 0 &&     // alone in the current map
-		signature->getNeighbors().size() == 0 &&           // no neighbors, alone in the current map
+		_memory->getWorkingMem().size()>1)       // The working memory should not be empty
-		_memory->getWorkingMem().size()>1)                 // The working memory should not be empty
 	{
 		_memory->deleteLocation(signature->id());
 	}
@@ -1751,9 +1757,9 @@ bool Rtabmap::process(const SensorData & data)
 	return true;
 }
 
-bool Rtabmap::process(const cv::Mat & sensorData, int id)
+bool Rtabmap::process(const cv::Mat & image, int id)
 {
-	return this->process(SensorData(sensorData, id));
+	return this->process(SensorData(image, id));
 }
 
 // SETTERS
@@ -1908,7 +1914,7 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
 					//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
 					UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
 					poses.insert(std::make_pair(ids[ind[i]], tmp));
-					if(fromS->getNeighbors().find(ids[ind[i]]) == fromS->getNeighbors().end() && // can't be a neighbor
+					if(fromS->getLinks().find(ids[ind[i]]) == fromS->getLinks().end() && // can't be a neighbor
 					   (minDistance == -1 || minDistance > dist[i]))
 					{
 						nearestId = ids[ind[i]];
@@ -2001,7 +2007,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		else
 		{
-			util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true);
+			util3d::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
 		}
 	}
 }
@@ -2030,7 +2036,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 	UDEBUG("values.size=%d", values.size());
 
 	float mean = uMean(values);
-	float stdDev = uStdDev(values, mean);
+	float stdDev = std::sqrt(uVariance(values, mean));
 
 
 	//Adjust likelihood with mean and standard deviation (see Angeli phd)
@@ -2132,8 +2138,15 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 			}
+
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			}
 		}
 
+
+		// Get data
 		std::set<int> ids = _memory->getWorkingMem(); // STM + WM
 
 		//remove virtual signature
@@ -2151,7 +2164,6 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			if(data.id() != Memory::kIdInvalid)
 			{
 				signatures.insert(std::make_pair(*iter, Signature())).first->second = data;
-				mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 			}
 		}
 	}
@@ -2185,6 +2197,11 @@ void Rtabmap::getGraph(
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
 			}
+
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			}
 		}
 		else
 		{
@@ -2197,11 +2214,6 @@ void Rtabmap::getGraph(
 		{
 			ids = _memory->getAllSignatureIds(); // STM + WM + LTM
 		}
-
-		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
-		{
-			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
-		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
 	{

corelib/src/SensorData.cpp

@@ -42,7 +42,8 @@ SensorData::SensorData() :
 	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
-	_localTransform(Transform::getIdentity())
+	_localTransform(Transform::getIdentity()),
+	_poseVariance(1.0f)
 {
 }
 
@@ -54,7 +55,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
-	_localTransform(Transform::getIdentity())
+	_localTransform(Transform::getIdentity()),
+	_poseVariance(1.0f)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -67,8 +69,9 @@ SensorData::SensorData(const cv::Mat & image,
 		  float fyOrBaseline,
 		  float cx,
 		  float cy,
-		  const Transform & pose,
 		  const Transform & localTransform,
+		  const Transform & pose,
+		  float poseVariance,
 		  int id) :
 	_image(image),
 	_id(id),
@@ -78,7 +81,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform)
+	_localTransform(localTransform),
+	_poseVariance(poseVariance)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -90,27 +94,30 @@ SensorData::SensorData(const cv::Mat & image,
 }
 
 	// Metric constructor + 2d depth
-SensorData::SensorData(const cv::Mat & image,
+SensorData::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) :
 	_image(image),
 	_id(id),
 	_depthOrRightImage(depthOrRightImage),
-	_depth2d(depth2d),
+	_laserScan(laserScan),
 	_fx(fx),
 	_fyOrBaseline(fyOrBaseline),
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform)
+	_localTransform(localTransform),
+	_poseVariance(poseVariance)
 {
+	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
 	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter

corelib/src/Signature.cpp

@@ -42,7 +42,7 @@ Signature::Signature() :
 	_weight(-1),
 	_saved(false),
 	_modified(true),
-	_neighborsModified(true),
+	_linksModified(true),
 	_enabled(false),
 	_fx(0.0f),
 	_fy(0.0f),
@@ -57,7 +57,7 @@ Signature::Signature(
 		const std::multimap<int, cv::KeyPoint> & words,
 		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
-		const cv::Mat & depth2DCompressed, // in base_link frame
+		const cv::Mat & laserScanCompressed, // in base_link frame
 		const cv::Mat & imageCompressed, // in camera_link frame
 		const cv::Mat & depthCompressed, // in camera_link frame
 		float fx,
@@ -70,12 +70,12 @@ Signature::Signature(
 	_weight(0),
 	_saved(false),
 	_modified(true),
-	_neighborsModified(true),
+	_linksModified(true),
 	_words(words),
 	_enabled(false),
 	_imageCompressed(imageCompressed),
 	_depthCompressed(depthCompressed),
-	_depth2DCompressed(depth2DCompressed),
+	_laserScanCompressed(laserScanCompressed),
 	_fx(fx),
 	_fy(fy),
 	_cx(cx),
@@ -91,80 +91,64 @@ Signature::~Signature()
 	//UDEBUG("id=%d", _id);
 }
 
-void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
+void Signature::addLinks(const std::list<Link> & links)
 {
-	for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 	{
-		this->addNeighbor(i->first, i->second);
+		addLink(*iter);
+	}
+}
+void Signature::addLinks(const std::map<int, Link> & links)
+{
+	for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+	{
+		addLink(iter->second);
+	}
+}
+void Signature::addLink(const Link & link)
+{
+	UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
+	UASSERT(link.from() == this->id());
+	std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
+	UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
+	_linksModified = true;
+}
+
+bool Signature::hasLink(int idTo) const
+{
+	return _links.find(idTo) != _links.end();
+}
+
+void Signature::changeLinkIds(int idFrom, int idTo)
+{
+	std::map<int, Link>::iterator iter = _links.find(idFrom);
+	if(iter != _links.end())
+	{
+		Link link = iter->second;
+		_links.erase(iter);
+		link.setTo(idTo);
+		_links.insert(std::make_pair(idTo, link));
+		_linksModified = true;
+		UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
 	}
 }
 
-void Signature::addNeighbor(int neighbor, const Transform & transform)
+void Signature::removeLinks()
 {
-	UDEBUG("Add neighbor %d to %d", neighbor, this->id());
+	if(_links.size())
-	_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
+		_linksModified = true;
-	_neighborsModified = true;
+	_links.clear();
 }
 
-void Signature::removeNeighbor(int neighborId)
+void Signature::removeLink(int idTo)
 {
-	int count = (int)_neighbors.erase(neighborId);
+	int count = (int)_links.erase(idTo);
 	if(count)
 	{
-		_neighborsModified = true;
+		_linksModified = true;
 	}
 }
 
-void Signature::removeNeighbors()
-{
-	if(_neighbors.size())
-		_neighborsModified = true;
-	_neighbors.clear();
-}
-
-void Signature::changeNeighborIds(int idFrom, int idTo)
-{
-	std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
-	if(iter != _neighbors.end())
-	{
-		Transform t = iter->second;
-		_neighbors.erase(iter);
-		_neighbors.insert(std::pair<int, Transform>(idTo, t));
-		_neighborsModified = true;
-	}
-	UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
-}
-
-void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
-{
-	if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
-	{
-		_neighborsModified=true;
-	}
-}
-
-void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
-{
-	if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
-	{
-		_neighborsModified=true;
-	}
-}
-
-void Signature::changeLoopClosureId(int idFrom, int idTo)
-{
-	std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
-	if(iter != _loopClosureIds.end())
-	{
-		Transform t = iter->second;
-		_loopClosureIds.erase(iter);
-		_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
-		_neighborsModified = true;
-	}
-	UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
-}
-
-
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
@@ -230,26 +214,43 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
 SensorData Signature::toSensorData()
 {
 	this->uncompressData();
-	return SensorData(_imageRaw,
+	float variance = 1.0f;
+	if(_links.size())
+	{
+		for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
+		{
+			if(iter->second.kNeighbor)
+			{
+				//Assume the first neighbor to be the backward neighbor link
+				if(iter->second.to() < iter->second.from())
+				{
+					variance = iter->second.variance();
+					break;
+				}
+			}
+		}
+	}
+	return SensorData(_laserScanRaw,
+			_imageRaw,
 			_depthRaw,
-			_depth2DRaw,
 			_fx,
 			_fy,
 			_cx,
 			_cy,
-			_pose,
 			_localTransform,
+			_pose,
+			variance,
 			_id);
 }
 
 void Signature::uncompressData()
 {
-	uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
+	uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
 }
 
-void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw)
+void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
 {
-	uncompressDataConst(imageRaw, depthRaw, depth2DRaw);
+	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
 		_imageRaw = *imageRaw;
@@ -258,13 +259,13 @@ void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat *
 	{
 		_depthRaw = *depthRaw;
 	}
-	if(depth2DRaw && !depth2DRaw->empty() && _depth2DRaw.empty())
+	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
 	{
-		_depth2DRaw = *depth2DRaw;
+		_laserScanRaw = *laserScanRaw;
 	}
 }
 
-void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const
+void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
 {
 	if(imageRaw)
 	{
@@ -274,17 +275,17 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 	{
 		*depthRaw = _depthRaw;
 	}
-	if(depth2DRaw)
+	if(laserScanRaw)
 	{
-		*depth2DRaw = _depth2DRaw;
+		*laserScanRaw = _laserScanRaw;
 	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
-		(depth2DRaw && depth2DRaw->empty()))
+		(laserScanRaw && laserScanRaw->empty()))
 	{
 		util3d::CompressionThread ctImage(_imageCompressed, true);
 		util3d::CompressionThread ctDepth(_depthCompressed, true);
-		util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
+		util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
 		if(imageRaw && imageRaw->empty())
 		{
 			ctImage.start();
@@ -293,13 +294,13 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 		{
 			ctDepth.start();
 		}
-		if(depth2DRaw && depth2DRaw->empty())
+		if(laserScanRaw && laserScanRaw->empty())
 		{
-			ctDepth2D.start();
+			ctLaserScan.start();
 		}
 		ctImage.join();
 		ctDepth.join();
-		ctDepth2D.join();
+		ctLaserScan.join();
 		if(imageRaw && imageRaw->empty())
 		{
 			*imageRaw = ctImage.getUncompressedData();
@@ -308,9 +309,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 		{
 			*depthRaw = ctDepth.getUncompressedData();
 		}
-		if(depth2DRaw && depth2DRaw->empty())
+		if(laserScanRaw && laserScanRaw->empty())
 		{
-			*depth2DRaw = ctDepth2D.getUncompressedData();
+			*laserScanRaw = ctLaserScan.getUncompressedData();
 		}
 	}
 }

corelib/src/resources/DatabaseSchema.sql.in

@@ -46,6 +46,7 @@ CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
 	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
+	variance FLOAT NOT NULL,
 	transform BLOB,
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)

corelib/src/util3d.cpp

@@ -1070,27 +1070,27 @@ cv::Mat depthFromDisparity(const cv::Mat & disparity,
 	return depth;
 }
 
-cv::Mat depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
 {
-	cv::Mat depth2d(1, (int)cloud.size(), CV_32FC2);
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		depth2d.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
-		depth2d.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
 	}
-	return depth2d;
+	return laserScan;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr depth2DToPointCloud(const cv::Mat & depth2D)
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
 {
-	UASSERT(depth2D.empty() || depth2D.type() == CV_32FC2);
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	output->resize(depth2D.cols);
+	output->resize(laserScan.cols);
-	for(int i=0; i<depth2D.cols; ++i)
+	for(int i=0; i<laserScan.cols; ++i)
 	{
-		output->at(i).x = depth2D.at<cv::Vec2f>(i)[0];
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-		output->at(i).y = depth2D.at<cv::Vec2f>(i)[1];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
 	}
 	return output;
 }
@@ -1495,12 +1495,17 @@ Transform transformFromXYZCorrespondences(
 		bool refineModel,
 		double refineModelSigma,
 		int refineModelIterations,
-		std::vector<int> * inliersOut)
+		std::vector<int> * inliersOut,
+		double * varianceOut)
 {
 	//NOTE: this method is a mix of two methods:
 	//  - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
 	//  - refineModel() in pcl/sample_consensus/sac.h
 
+	if(varianceOut)
+	{
+		*varianceOut = 1.0f;
+	}
 	Transform transform;
 	if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
 	{
@@ -1626,6 +1631,10 @@ Transform transformFromXYZCorrespondences(
 				{
 					*inliersOut = inliers;
 				}
+				if(varianceOut)
+				{
+					*varianceOut = model->computeVariance();
+				}
 
 				// get best transformation
 				Eigen::Matrix4f bestTransformation;
@@ -1661,8 +1670,9 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool & hasConverged,
+			  bool * hasConvergedOut,
-			  double & fitnessScore)
+			  double * variance,
+			  int * inliers)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -1677,13 +1687,65 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setTransformationEpsilon (transformationEpsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
-	icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
+	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
-	icp.align (cloud_source_registered);
+	icp.align (*cloud_source_registered);
-	fitnessScore = icp.getFitnessScore();
+	bool hasConverged = icp.hasConverged();
-	hasConverged = icp.hasConverged();
+
+	// compute variance
+	if((inliers || variance) && hasConverged)
+	{
+		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
+		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+		est->setInputTarget(cloud_target);
+		est->setInputSource(cloud_source_registered);
+		pcl::Correspondences correspondences;
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+		if(variance)
+		{
+			if(correspondences.size()>=3)
+			{
+				std::vector<double> distances(correspondences.size());
+				for(unsigned int i=0; i<correspondences.size(); ++i)
+				{
+					distances[i] = correspondences[i].distance;
+				}
+
+				//variance
+				std::sort(distances.begin (), distances.end ());
+				double median_error_sqr = distances[distances.size () >> 1];
+				*variance = (2.1981 * median_error_sqr);
+			}
+			else
+			{
+				hasConverged = false;
+				*variance = -1.0;
+			}
+		}
+
+		if(inliers)
+		{
+			*inliers = correspondences.size();
+		}
+	}
+	else
+	{
+		if(inliers)
+		{
+			*inliers = 0;
+		}
+		if(variance)
+		{
+			*variance = -1;
+		}
+	}
+
+	if(hasConvergedOut)
+	{
+		*hasConvergedOut = hasConverged;
+	}
 
 	return transformFromEigen4f(icp.getFinalTransformation());
 }
@@ -1694,8 +1756,9 @@ Transform icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool & hasConverged,
+		bool * hasConvergedOut,
-		double & fitnessScore)
+		double * variance,
+		int * inliers)
 {
 	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
 	// Set the input source and target
@@ -1714,13 +1777,65 @@ Transform icpPointToPlane(
 	//icp.setTransformationEpsilon (transformationEpsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
-	icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
+	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointNormal> cloud_source_registered;
+	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
-	icp.align (cloud_source_registered);
+	icp.align (*cloud_source_registered);
-	fitnessScore = icp.getFitnessScore();
+	bool hasConverged = icp.hasConverged();
-	hasConverged = icp.hasConverged();
+
+	// compute variance
+	if((inliers || variance) && hasConverged)
+	{
+		pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
+		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
+		est->setInputTarget(cloud_target);
+		est->setInputSource(cloud_source_registered);
+		pcl::Correspondences correspondences;
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+		if(variance)
+		{
+			if(correspondences.size()>=3)
+			{
+				std::vector<double> distances(correspondences.size());
+				for(unsigned int i=0; i<correspondences.size(); ++i)
+				{
+					distances[i] = correspondences[i].distance;
+				}
+
+				//variance
+				std::sort(distances.begin (), distances.end ());
+				double median_error_sqr = distances[distances.size () >> 1];
+				*variance = (2.1981 * median_error_sqr);
+			}
+			else
+			{
+				hasConverged = false;
+				*variance = -1.0;
+			}
+		}
+
+		if(inliers)
+		{
+			*inliers = correspondences.size();
+		}
+	}
+	else
+	{
+		if(inliers)
+		{
+			*inliers = 0;
+		}
+		if(variance)
+		{
+			*variance = -1;
+		}
+	}
+
+	if(hasConvergedOut)
+	{
+		*hasConvergedOut = hasConverged;
+	}
 
 	return transformFromEigen4f(icp.getFinalTransformation());
 }
@@ -1730,8 +1845,9 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool & hasConverged,
+			  bool * hasConvergedOut,
-			  double & fitnessScore)
+			  double * variance,
+			  int * inliers)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -1750,13 +1866,65 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setTransformationEpsilon (transformationEpsilon);
 	// Set the euclidean distance difference epsilon (criterion 3)
 	//icp.setEuclideanFitnessEpsilon (1);
-	icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
+	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ> cloud_source_registered;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
-	icp.align (cloud_source_registered);
+	icp.align (*cloud_source_registered);
-	fitnessScore = icp.getFitnessScore();
+	bool hasConverged = icp.hasConverged();
-	hasConverged = icp.hasConverged();
+
+	// compute variance
+	if((inliers || variance) && hasConverged)
+	{
+		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
+		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+		est->setInputTarget(cloud_target);
+		est->setInputSource(cloud_source_registered);
+		pcl::Correspondences correspondences;
+		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+		if(variance)
+		{
+			if(correspondences.size()>=3)
+			{
+				std::vector<double> distances(correspondences.size());
+				for(unsigned int i=0; i<correspondences.size(); ++i)
+				{
+					distances[i] = correspondences[i].distance;
+				}
+
+				//variance
+				std::sort(distances.begin (), distances.end ());
+				double median_error_sqr = distances[distances.size () >> 1];
+				*variance = (2.1981 * median_error_sqr);
+			}
+			else
+			{
+				hasConverged = false;
+				*variance = -1.0;
+			}
+		}
+
+		if(inliers)
+		{
+			*inliers = correspondences.size();
+		}
+	}
+	else
+	{
+		if(inliers)
+		{
+			*inliers = 0;
+		}
+		if(variance)
+		{
+			*variance = -1;
+		}
+	}
+
+	if(hasConvergedOut)
+	{
+		*hasConvergedOut = hasConverged;
+	}
 
 	return transformFromEigen4f(icp.getFinalTransformation());
 }
@@ -2145,6 +2313,7 @@ void optimizeTOROGraph(
 		std::map<int, Transform> & optimizedPoses,
 		int toroIterations,
 		bool toroInitialGuess,
+		bool ignoreCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes)
 {
 	optimizedPoses.clear();
@@ -2190,17 +2359,26 @@ void optimizeTOROGraph(
 		{
 			if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
 			{
-				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
+				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
 			}
 		}
 
 		std::map<int, rtabmap::Transform> optimizedPosesToro;
 
-		// Optimize!
 		if(posesToro.size() && edgeConstraintsToro.size())
 		{
 			std::list<std::map<int, rtabmap::Transform> > graphesToro;
-			rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, toroIterations, toroInitialGuess, &graphesToro);
+
+			// Optimize!
+			rtabmap::util3d::optimizeTOROGraph(
+					posesToro,
+					edgeConstraintsToro,
+					optimizedPosesToro,
+					toroIterations,
+					toroInitialGuess,
+					ignoreCovariance,
+					&graphesToro);
+
 			for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
 			{
 				optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
@@ -2242,6 +2420,7 @@ void optimizeTOROGraph(
 		std::map<int, Transform> & optimizedPoses,
 		int toroIterations,
 		bool toroInitialGuess,
+		bool ignoreCovariance,
 		std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
 {
 	UASSERT(toroIterations>0);
@@ -2274,13 +2453,21 @@ void optimizeTOROGraph(
 			float x,y,z, roll,pitch,yaw;
 			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll,pitch,yaw);
 			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
-			AISNavigation::TreePoseGraph3::InformationMatrix m;
+			AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
-			m=DMatrix<double>::I(6);
+			if(!ignoreCovariance && iter->second.variance()>0)
+			{
+				inf[0][0] = 1.0f/iter->second.variance(); // x
+				inf[1][1] = 1.0f/iter->second.variance(); // y
+				inf[2][2] = 1.0f/iter->second.variance(); // z
+				inf[3][3] = 1.0f/iter->second.variance(); // roll
+				inf[4][4] = 1.0f/iter->second.variance(); // pitch
+				inf[5][5] = 1.0f/iter->second.variance(); // yaw
+			}
 
 			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
 			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
 			AISNavigation::TreePoseGraph3::Transformation t(p);
-			if (!pg.addEdge(v1, v2,t ,m))
+			if (!pg.addEdge(v1, v2, t, inf))
 			{
 				UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 				return;
@@ -2380,7 +2567,7 @@ bool saveTOROGraph(
 		{
 			float x,y,z, yaw,pitch,roll;
 			pcl::getTranslationAndEulerAngles(transformToEigen3f(iter->second.transform()), x,y,z, roll, pitch, yaw);
-			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1\n",
+			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",
 					iter->first,
 					iter->second.to(),
 					x,
@@ -2388,7 +2575,13 @@ bool saveTOROGraph(
 					z,
 					roll,
 					pitch,
-					yaw);
+					yaw,
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance(),
+					1.0f/iter->second.variance());
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
 		fclose(file);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -95,6 +95,7 @@ private slots:
 	void addConstraint();
 	void resetConstraint();
 	void rejectConstraint();
+	void updateConstraintView();
 
 private:
 	void updateIds();
@@ -120,9 +121,9 @@ private:
 	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
 			const std::multimap<int, rtabmap::Link> & edgeConstraints);
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
-	void refineConstraint(int from, int to);
+	void refineConstraint(int from, int to, bool updateGraph);
-	void refineConstraintVisually(int from, int to);
+	void refineConstraintVisually(int from, int to, bool updateGraph);
-	bool addConstraint(int from, int to, bool silent);
+	bool addConstraint(int from, int to, bool silent, bool updateGraph);
 
 private:
 	Ui_DatabaseViewer * ui_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -35,6 +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/gui/PreferencesDialog.h"
 
 #include <pcl/point_cloud.h>
@@ -148,7 +149,7 @@ private slots:
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
 	void updateElapsedTime();
-	void processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize);
+	void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
 	void processRtabmapEventInit(int status, const QString & info);
@@ -179,7 +180,7 @@ private slots:
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
-	void odometryReceived(const rtabmap::SensorData &, int, float, int, int);
+	void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);

guilib/src/DatabaseViewer.cpp

@@ -137,6 +137,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
 	connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
 	connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
+	connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
+	ui_->checkBox_showOptimized->setEnabled(false);
 
 	ui_->horizontalSlider_iterations->setTracking(false);
 	ui_->dockWidget_graphView->setEnabled(false);
@@ -145,9 +147,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 
 	ui_->constraintsViewer->setCameraLockZ(false);
-	ui_->constraintsViewer->updateCameraPosition(Transform::getIdentity());
+	ui_->constraintsViewer->setCameraFree();
 }
 
 DatabaseViewer::~DatabaseViewer()
@@ -189,6 +192,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			linksRemoved_.clear();
 			scans_.clear();
 			ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
+			ui_->checkBox_showOptimized->setEnabled(false);
 		}
 
 		std::string driverType = "sqlite3";
@@ -238,11 +242,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				std::multimap<int, rtabmap::Link>::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
-					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true);
+					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
 				}
 				else
 				{
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
+					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
@@ -252,7 +256,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
 				{
 					memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
-					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
+					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
 				}
 			}
 
@@ -550,6 +554,128 @@ void DatabaseViewer::generateTOROGraph()
 }
 
 void DatabaseViewer::view3DMap()
+{
+	if(!ids_.size() || !memory_)
+	{
+		QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
+		return;
+	}
+	if(graphes_.empty())
+	{
+		this->updateGraphView();
+		if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
+		{
+			QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
+			return;
+		}
+	}
+	bool ok = false;
+	QStringList items;
+	items.append("1");
+	items.append("2");
+	items.append("4");
+	items.append("8");
+	items.append("16");
+	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
+	if(ok)
+	{
+		int decimation = item.toInt();
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
+		if(ok)
+		{
+			const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+			if(optimizedPoses.size() > 0)
+			{
+				rtabmap::DetailedProgressDialog progressDialog(this);
+				progressDialog.setMaximumSteps(optimizedPoses.size());
+				progressDialog.show();
+
+				// create a window
+				QDialog * window = new QDialog(this, Qt::Window);
+				window->setModal(this->isModal());
+				window->setWindowTitle(tr("3D Map"));
+				window->setMinimumWidth(800);
+				window->setMinimumHeight(600);
+
+				rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
+
+				QVBoxLayout *layout = new QVBoxLayout();
+				layout->addWidget(viewer);
+				viewer->setCameraLockZ(false);
+				window->setLayout(layout);
+				connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
+
+				window->show();
+
+				for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+				{
+					rtabmap::Transform pose = iter->second;
+					if(!pose.isNull())
+					{
+						Signature data = memory_->getSignatureData(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.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
+						if(data.getDepthRaw().type() == CV_8UC1)
+						{
+							cv::Mat leftImg;
+							if(data.getImageRaw().channels() == 3)
+							{
+								cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
+							}
+							else
+							{
+								leftImg = data.getImageRaw();
+							}
+							cloud = rtabmap::util3d::cloudFromDisparityRGB(
+									data.getImageRaw(),
+								util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
+								data.getDepthCx(), data.getDepthCy(),
+								data.getDepthFx(), data.getDepthFy(),
+								decimation);
+						}
+						else
+						{
+							cloud = rtabmap::util3d::cloudFromDepthRGB(
+									data.getImageRaw(),
+									data.getDepthRaw(),
+									data.getDepthCx(), data.getDepthCy(),
+									data.getDepthFx(), data.getDepthFy(),
+									decimation);
+						}
+
+						if(maxDepth)
+						{
+							cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
+						}
+
+						cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
+
+						QColor color = Qt::red;
+						int mapId = memory_->getMapId(iter->first);
+						if(mapId >= 0)
+						{
+							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();
+					}
+				}
+				progressDialog.setValue(progressDialog.maximumSteps());
+			}
+			else
+			{
+				QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
+			}
+		}
+	}
+}
+
+void DatabaseViewer::generate3DMap()
 {
 	if(!ids_.size() || !memory_)
 	{
@@ -557,58 +683,32 @@ void DatabaseViewer::view3DMap()
 		return;
 	}
 	bool ok = false;
-	int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
+	QStringList items;
+	items.append("1");
+	items.append("2");
+	items.append("4");
+	items.append("8");
+	items.append("16");
+	QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 	if(ok)
 	{
-		QStringList items;
+		int decimation = item.toInt();
-		items.append("1");
+		double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
-		items.append("2");
-		items.append("4");
-		items.append("8");
-		items.append("16");
-		QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
 		if(ok)
 		{
-			int decimation = item.toInt();
+			QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
-			double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
+			if(!path.isEmpty())
-			if(ok)
 			{
-				std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
+				const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
-				// <id, depth>
+				if(optimizedPoses.size() > 0)
-				std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), margin);
-				if(depthGraph.size() > 0)
 				{
-					rtabmap::DetailedProgressDialog progressDialog(this);
+					rtabmap::DetailedProgressDialog progressDialog;
-					progressDialog.setMaximumSteps(depthGraph.size()+2);
+					progressDialog.setMaximumSteps((int)optimizedPoses.size());
 					progressDialog.show();
 
-					progressDialog.appendText("Graph optimization...");
+					for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
-					std::multimap<int, Link> links = updateLinksWithModifications(links_);
-					std::map<int, Transform> optimizedPoses;
-					util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
-					progressDialog.appendText("Graph optimization... done!");
-					progressDialog.incrementStep();
-
-					// create a window
-					QDialog * window = new QDialog(this, Qt::Window);
-					window->setModal(this->isModal());
-					window->setWindowTitle(tr("3D Map"));
-					window->setMinimumWidth(800);
-					window->setMinimumHeight(600);
-
-					rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
-
-					QVBoxLayout *layout = new QVBoxLayout();
-					layout->addWidget(viewer);
-					viewer->setCameraLockZ(false);
-					window->setLayout(layout);
-					connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
-
-					window->show();
-
-					for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 					{
-						rtabmap::Transform pose = iter->second;
+						const rtabmap::Transform & pose = iter->second;
 						if(!pose.isNull())
 						{
 							Signature data = memory_->getSignatureData(iter->first, true);
@@ -648,23 +748,18 @@ void DatabaseViewer::view3DMap()
 								cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 							}
 
-							cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
+							cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
-
+							std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
-							QColor color = Qt::red;
+							pcl::io::savePCDFile(name, *cloud);
-							int mapId = memory_->getMapId(iter->first);
+							UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
-							if(mapId >= 0)
+							progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
-							{
-								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();
 						}
 					}
 					progressDialog.setValue(progressDialog.maximumSteps());
+
+					QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
 				}
 				else
 				{
@@ -675,132 +770,9 @@ void DatabaseViewer::view3DMap()
 	}
 }
 
-void DatabaseViewer::generate3DMap()
-{
-	if(!ids_.size() || !memory_)
-	{
-		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
-		return;
-	}
-	bool ok = false;
-	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
-
-	if(ok)
-	{
-		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
-		if(ok)
-		{
-			QStringList items;
-			items.append("1");
-			items.append("2");
-			items.append("4");
-			items.append("8");
-			items.append("16");
-			QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
-			if(ok)
-			{
-				int decimation = item.toInt();
-				double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
-				if(ok)
-				{
-					QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
-					if(!path.isEmpty())
-					{
-						std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-						// <id, depth>
-						std::map<int, int> depthGraph = util3d::generateDepthGraph(links, id, margin);
-						if(depthGraph.size() > 0)
-						{
-							rtabmap::DetailedProgressDialog progressDialog;
-							progressDialog.setMaximumSteps((int)depthGraph.size()+2);
-							progressDialog.show();
-
-							progressDialog.appendText("Graph generation...");
-							std::map<int, rtabmap::Transform> poses, optimizedPoses;
-							std::multimap<int, rtabmap::Link> edgeConstraints;
-							memory_->getMetricConstraints(uKeys(depthGraph), poses, edgeConstraints, true);
-							edgeConstraints = updateLinksWithModifications(edgeConstraints);
-							progressDialog.appendText("Graph generation... done!");
-							progressDialog.incrementStep();
-
-							progressDialog.appendText("Graph optimization...");
-							rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
-							progressDialog.appendText("Graph optimization... done!");
-							progressDialog.incrementStep();
-
-							for(std::map<int, int>::iterator iter = depthGraph.begin(); iter!=depthGraph.end(); ++iter)
-							{
-								rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
-								if(!pose.isNull())
-								{
-									Signature data = memory_->getSignatureData(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.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
-									if(data.getDepthRaw().type() == CV_8UC1)
-									{
-										cv::Mat leftImg;
-										if(data.getImageRaw().channels() == 3)
-										{
-											cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
-										}
-										else
-										{
-											leftImg = data.getImageRaw();
-										}
-										cloud = rtabmap::util3d::cloudFromDisparityRGB(
-												data.getImageRaw(),
-											util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
-											data.getDepthCx(), data.getDepthCy(),
-											data.getDepthFx(), data.getDepthFy(),
-											decimation);
-									}
-									else
-									{
-										cloud = rtabmap::util3d::cloudFromDepthRGB(
-												data.getImageRaw(),
-												data.getDepthRaw(),
-												data.getDepthCx(), data.getDepthCy(),
-												data.getDepthFx(), data.getDepthFy(),
-												decimation);
-									}
-
-									if(maxDepth)
-									{
-										cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
-									}
-
-									cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(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();
-								}
-							}
-							progressDialog.setValue(progressDialog.maximumSteps());
-
-							QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(depthGraph.size()).arg(path));
-						}
-						else
-						{
-							QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(id));
-						}
-					}
-				}
-			}
-		}
-	}
-}
-
 void DatabaseViewer::detectMoreLoopClosures()
 {
-	std::map<int, rtabmap::Transform> optimizedPoses;
+	const std::map<int, Transform> & optimizedPoses = graphes_.back();
-
-	std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
-	std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value());
-	util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
 
 	int iterations = ui_->doubleSpinBox_detectMore_iterations->value();
 	UASSERT(iterations > 0);
@@ -825,7 +797,7 @@ void DatabaseViewer::detectMoreLoopClosures()
 			if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
 				addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
 			{
-				if(addConstraint(from, to, true))
+				if(addConstraint(from, to, true, false))
 				{
 					UINFO("Added new loop closure between %d and %d.", from, to);
 					++added;
@@ -840,6 +812,10 @@ void DatabaseViewer::detectMoreLoopClosures()
 			break;
 		}
 	}
+	if(added)
+	{
+		this->updateGraphView();
+	}
 	UINFO("Total added %d loop closures.", added);
 }
 
@@ -855,12 +831,14 @@ void DatabaseViewer::refineAllNeighborLinks()
 		{
 			int from = neighborLinks_[i].from();
 			int to = neighborLinks_[i].to();
-			this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to());
+			this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
 			progressDialog.incrementStep();
 			QApplication::processEvents();
 		}
+		this->updateGraphView();
+
 		progressDialog.setValue(progressDialog.maximumSteps());
 		progressDialog.appendText("Refining links finished!");
 	}
@@ -878,12 +856,14 @@ void DatabaseViewer::refineAllLoopClosureLinks()
 		{
 			int from = loopLinks_[i].from();
 			int to = loopLinks_[i].to();
-			this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to());
+			this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
 			progressDialog.incrementStep();
 			QApplication::processEvents();
 		}
+		this->updateGraphView();
+
 		progressDialog.setValue(progressDialog.maximumSteps());
 		progressDialog.appendText("Refining links finished!");
 	}
@@ -901,12 +881,14 @@ void DatabaseViewer::refineVisuallyAllNeighborLinks()
 		{
 			int from = neighborLinks_[i].from();
 			int to = neighborLinks_[i].to();
-			this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to());
+			this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
 			progressDialog.incrementStep();
 			QApplication::processEvents();
 		}
+		this->updateGraphView();
+
 		progressDialog.setValue(progressDialog.maximumSteps());
 		progressDialog.appendText("Refining links finished!");
 	}
@@ -924,12 +906,14 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
 		{
 			int from = loopLinks_[i].from();
 			int to = loopLinks_[i].to();
-			this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to());
+			this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), false);
 
 			progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
 			progressDialog.incrementStep();
 			QApplication::processEvents();
 		}
+		this->updateGraphView();
+
 		progressDialog.setValue(progressDialog.maximumSteps());
 		progressDialog.appendText("Refining links finished!");
 	}
@@ -1026,26 +1010,24 @@ void DatabaseViewer::update(int value,
 			}
 
 			// loops
-			std::map<int, rtabmap::Transform> parents;
+			std::map<int, rtabmap::Link> loopClosures;
-			std::map<int, rtabmap::Transform> children;
+			loopClosures = memory_->getLoopClosureLinks(id, true);
-			memory_->getLoopClosureIds(id, parents, children, true);
+			if(loopClosures.size())
-			if(parents.size())
 			{
-				QString str;
+				QString strParents, strChildren;
-				for(std::map<int, rtabmap::Transform>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
+				for(std::map<int, rtabmap::Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end(); ++iter)
 				{
-					str.append(QString("%1 ").arg(iter->first));
+					if(iter->first < id)
+					{
+						strChildren.append(QString("%1 ").arg(iter->first));
+					}
+					else
+					{
+						strParents.append(QString("%1 ").arg(iter->first));
+					}
 				}
-				labelParents->setText(str);
+				labelParents->setText(strParents);
-			}
+				labelChildren->setText(strChildren);
-			if(children.size())
-			{
-				QString str;
-				for(std::map<int, rtabmap::Transform>::iterator iter=children.begin(); iter!=children.end(); ++iter)
-				{
-					str.append(QString("%1 ").arg(iter->first));
-				}
-				labelChildren->setText(str);
 			}
 		}
 
@@ -1396,22 +1378,61 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
 	this->updateConstraintView(loopLinks_.at(value));
 }
 
-void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
+// only called when ui_->checkBox_showOptimized state changed
+void DatabaseViewer::updateConstraintView()
+{
+	this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()),
+			pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
+			pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
+			false);
+}
+
+void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
 		bool updateImageSliders)
 {
-	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, link.from(), link.to());
+	std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, linkIn.from(), linkIn.to());
-	rtabmap::Transform t = link.transform();
+	rtabmap::Link link = linkIn;
 	if(iter != linksRefined_.end())
 	{
-		t = iter->second.transform();
+		link = iter->second;
 	}
+	rtabmap::Transform t = link.transform();
 
 	ui_->label_constraint->clear();
+	ui_->label_constraint_opt->clear();
+	ui_->checkBox_showOptimized->setEnabled(false);
 	UASSERT(!t.isNull() && memory_);
 
-	ui_->label_constraint->setText(t.prettyPrint().c_str());
+	ui_->label_constraint->setText(QString("%1 (%2=%3)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.variance())));
+	if(link.type() == Link::kNeighbor &&
+	   graphes_.size() &&
+	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
+	{
+		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+		if(link.type() == Link::kNeighbor)
+		{
+			std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
+			std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
+			if(iterFrom != graph.end() && iterTo != graph.end())
+			{
+				ui_->checkBox_showOptimized->setEnabled(true);
+				Transform topt = iterFrom->second.inverse()*iterTo->second;
+				Transform delta = t.inverse()*topt;
+				Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
+				Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
+				float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
+				a = (a *180.0f) / CV_PI;
+				ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((delta.getNorm()/t.getNorm())*100.0f).arg(a));
+
+				if(ui_->checkBox_showOptimized->isChecked())
+				{
+					t = topt;
+				}
+			}
+		}
+	}
 
 	if(updateImageSliders)
 	{
@@ -1587,8 +1608,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
+			scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
-			scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
+			scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
 			scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
 			if(scanA->size())
@@ -1611,6 +1632,11 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 				ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
 			}
 		}
+
+		//update cordinate
+		ui_->constraintsViewer->updateCameraPosition(t);
+		ui_->constraintsViewer->clearTrajectory();
+
 		ui_->constraintsViewer->render();
 	}
 
@@ -1669,18 +1695,18 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 {
 	if(memory_ && value >=0 && value < (int)graphes_.size())
 	{
-		if(scans_.size() == 0)
+		if(ui_->dockWidget_graphView->isVisible() && scans_.size() == 0)
 		{
 			//update scans
 			UINFO("Update scans list...");
 			for(int i=0; i<ids_.size(); ++i)
 			{
 				Signature data = memory_->getSignatureData(ids_.at(i), false);
-				if(!data.getDepth2DCompressed().empty())
+				if(!data.getLaserScanCompressed().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2DCompressed());
+					cv::Mat laserScan = rtabmap::util3d::uncompressData(data.getLaserScanCompressed());
-					cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
+					cloud = rtabmap::util3d::laserScanToPointCloud(laserScan);
 					scans_.insert(std::make_pair(ids_.at(i), cloud));
 				}
 			}
@@ -1722,7 +1748,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 }
 void DatabaseViewer::updateGraphView()
 {
-	if(ui_->dockWidget_graphView->isVisible() && poses_.size())
+	if(poses_.size())
 	{
 		if(!uContains(poses_, ui_->spinBox_optimizationsFrom->value()))
 		{
@@ -1740,7 +1766,14 @@ void DatabaseViewer::updateGraphView()
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
 		std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
 		std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
-		util3d::optimizeTOROGraph(depthGraph, poses_, links, finalPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphes_);
+		util3d::optimizeTOROGraph(
+				depthGraph,
+				poses_,
+				links, finalPoses,
+				ui_->spinBox_iterations->value(),
+				ui_->checkBox_initGuess->isChecked(),
+				ui_->checkBox_ignoreCovariance->isChecked(),
+				&graphes_);
 		graphes_.push_back(finalPoses);
 	}
 	if(graphes_.size())
@@ -1792,10 +1825,10 @@ void DatabaseViewer::refineConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraint(from, to);
+	refineConstraint(from, to, true);
 }
 
-void DatabaseViewer::refineConstraint(int from, int to)
+void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 {
 	if(from == to)
 	{
@@ -1809,10 +1842,29 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		UERROR("Not found link! (%d->%d)", from, to);
 		return;
 	}
+	Transform t = currentLink.transform();
+	if(ui_->checkBox_showOptimized->isChecked() &&
+	   currentLink.type() == Link::kNeighbor &&
+	   graphes_.size() &&
+	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
+	{
+		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+		if(currentLink.type() == Link::kNeighbor)
+		{
+			std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
+			std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
+			if(iterFrom != graph.end() && iterTo != graph.end())
+			{
+				Transform topt = iterFrom->second.inverse()*iterTo->second;
+				t = topt;
+			}
+		}
+	}
 
 
 	bool hasConverged = false;
-	double fitness = 0.0f;
+	double variance = -1.0;
+	int correspondences = 0;
 	Transform transform;
 
 	Signature dataFrom, dataTo;
@@ -1824,14 +1876,14 @@ void DatabaseViewer::refineConstraint(int from, int to)
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
-		cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2DCompressed());
+		cv::Mat oldLaserScan = util3d::uncompressData(dataFrom.getLaserScanCompressed());
-		cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2DCompressed());
+		cv::Mat newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed());
 
-		if(!oldDepth2D.empty() && !newDepth2D.empty())
+		if(!oldLaserScan.empty() && !newLaserScan.empty())
 		{
 			// 2D
-			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldLaserScan);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, currentLink.transform());
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newLaserScan, t);
 
 			//voxelize
 			if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
@@ -1846,8 +1898,9 @@ void DatabaseViewer::refineConstraint(int from, int to)
 						oldCloud,
 						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 						ui_->spinBox_icp_iteration->value(),
-					   hasConverged,
+					   &hasConverged,
-					   fitness);
+					   &variance,
+					   &correspondences);
 			}
 		}
 	}
@@ -1911,7 +1964,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
 			{
 				cloudB = util3d::voxelize<pcl::PointXYZ>(cloudB, ui_->doubleSpinBox_icp_voxel->value());
 			}
-			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, currentLink.transform() * dataTo.getLocalTransform());
+			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, t * dataTo.getLocalTransform());
 		}
 		else
 		{
@@ -1921,7 +1974,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
 					ui_->doubleSpinBox_icp_maxDepth->value(),
 					ui_->doubleSpinBox_icp_voxel->value(),
 					0, // no sampling
-					currentLink.transform() * dataTo.getLocalTransform());
+					t * dataTo.getLocalTransform());
 		}
 
 		if(ui_->checkBox_icp_p2plane->isChecked())
@@ -1945,8 +1998,9 @@ void DatabaseViewer::refineConstraint(int from, int to)
 					cloudANormals,
 					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 					ui_->spinBox_icp_iteration->value(),
-					hasConverged,
+					&hasConverged,
-					fitness);
+					&variance,
+					&correspondences);
 		}
 		else
 		{
@@ -1954,15 +2008,15 @@ void DatabaseViewer::refineConstraint(int from, int to)
 					cloudA,
 					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
 					ui_->spinBox_icp_iteration->value(),
-					hasConverged,
+					&hasConverged,
-					fitness);
+					&variance,
+					&correspondences);
 		}
 	}
 
 	if(hasConverged && !transform.isNull())
 	{
-		ui_->label_fitness->setNum(fitness);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance);
-		Link newLink(currentLink.from(), currentLink.to(), transform*currentLink.transform(), currentLink.type());
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -1980,27 +2034,29 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		if(!updated)
 		{
 			linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
+
+			if(updateGraph)
+			{
+				this->updateGraphView();
+			}
 		}
+
 		if(ui_->dockWidget_constraints->isVisible())
 		{
 			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
 			this->updateConstraintView(newLink, cloudA, cloudB);
 		}
 	}
-	else
-	{
-		ui_->label_fitness->setText("not converged");
-	}
 }
 
 void DatabaseViewer::refineConstraintVisually()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	refineConstraintVisually(from, to);
+	refineConstraintVisually(from, to, true);
 }
 
-void DatabaseViewer::refineConstraintVisually(int from, int to)
+void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph)
 {
 	if(from == to)
 	{
@@ -2017,6 +2073,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
 
 	Transform t;
 	std::string rejectedMsg;
+	double variance = -1.0;
+	int inliers = -1;
 	if(ui_->checkBox_visual_recomputeFeatures->isChecked())
 	{
 		// create a fake memory to regenerate features
@@ -2048,7 +2106,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
 		}
 
 
-		t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
+		t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
 	}
 	else
 	{
@@ -2058,12 +2116,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
 		parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
 		parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
 		memory_->parseParameters(parameters);
-		t = memory_->computeVisualTransform(to, from, &rejectedMsg);
+		t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
 	}
 
 	if(!t.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), t, currentLink.type());
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2081,6 +2139,11 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
 		if(!updated)
 		{
 			linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
+
+			if(updateGraph)
+			{
+				this->updateGraphView();
+			}
 		}
 		if(ui_->dockWidget_constraints->isVisible())
 		{
@@ -2093,10 +2156,10 @@ void DatabaseViewer::addConstraint()
 {
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	addConstraint(from, to, false);
+	addConstraint(from, to, false, true);
 }
 
-bool DatabaseViewer::addConstraint(int from, int to, bool silent)
+bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGraph)
 {
 	if(from < to)
 	{
@@ -2120,6 +2183,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 
 		Transform t;
 		std::string rejectedMsg;
+		double variance = -1.0;
+		int inliers = -1;
 		if(ui_->checkBox_visual_recomputeFeatures->isChecked())
 		{
 			// create a fake memory to regenerate features
@@ -2151,7 +2216,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			}
 
 
-			t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
+			t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
 		}
 		else
 		{
@@ -2161,7 +2226,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
 			parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
 			memory_->parseParameters(parameters);
-			t = memory_->computeVisualTransform(to, from, &rejectedMsg);
+			t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
 		}
 
 		if(t.isNull())
@@ -2184,7 +2249,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			}
 
 			// transform is valid, make a link
-			linksAdded_.insert(std::make_pair(from, Link(from, to, t, Link::kUserClosure)));
+			linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance)));
 			updateSlider = true;
 		}
 	}
@@ -2198,6 +2263,10 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 	if(updateSlider)
 	{
 		updateLoopClosuresSlider(from, to);
+		if(updateGraph)
+		{
+			this->updateGraphView();
+		}
 	}
 	return updateSlider;
 }
@@ -2224,6 +2293,7 @@ void DatabaseViewer::resetConstraint()
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
+		this->updateGraphView();
 	}
 
 	iter = util3d::findLink(links_, from, to);
@@ -2255,6 +2325,8 @@ void DatabaseViewer::rejectConstraint()
 		return;
 	}
 
+	bool removed = false;
+
 	// find the original one
 	std::multimap<int, Link>::iterator iter;
 	iter = util3d::findLink(links_, from, to);
@@ -2266,6 +2338,7 @@ void DatabaseViewer::rejectConstraint()
 			return;
 		}
 		linksRemoved_.insert(*iter);
+		removed = true;
 	}
 
 	// remove from refined and added
@@ -2273,11 +2346,17 @@ void DatabaseViewer::rejectConstraint()
 	if(iter != linksRefined_.end())
 	{
 		linksRefined_.erase(iter);
+		removed = true;
 	}
 	iter = util3d::findLink(linksAdded_, from, to);
 	if(iter != linksAdded_.end())
 	{
 		linksAdded_.erase(iter);
+		removed = true;
+	}
+	if(removed)
+	{
+		this->updateGraphView();
 	}
 	updateLoopClosuresSlider();
 }

guilib/src/LoopClosureViewer.cpp

@@ -169,8 +169,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = util3d::depth2DToPointCloud(sA_.getDepth2DRaw());
+			scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw());
-			scanB = util3d::depth2DToPointCloud(sB_.getDepth2DRaw());
+			scanB = util3d::laserScanToPointCloud(sB_.getLaserScanRaw());
 			scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
 			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));

guilib/src/MainWindow.cpp

@@ -358,7 +358,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
 
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
-	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float, int, int)), this, SLOT(processOdometry(rtabmap::SensorData, int, float, int, int)));
+	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
+	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
 
 	connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
 
@@ -570,7 +571,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		   !_processingStatistics)
 		{
 			_lastOdometryProcessed = false; // if we receive too many odometry events!
-			emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time(), odomEvent->features(), odomEvent->localMapSize());
+			emit odometryReceived(odomEvent->data(), odomEvent->info());
 		}
 	}
 	else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -580,7 +581,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		{
 			QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
 			// The timer prevents multiple calls to pauseDetection() before the state can be changed
-			if(_state != kPaused && _logEventTime->elapsed() > 1000)
+			if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
 			{
 				_logEventTime->start();
 				if(_preferencesDialog->beepOnPause())
@@ -593,7 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 }
 
-void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
+void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
 {
 	Transform pose = data.pose();
 	bool lost = false;
@@ -607,11 +608,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 		pose = _lastOdomPose;
 		lost = true;
 	}
-	else if(quality>=0 &&
+	else if(info.inliers>=0 &&
 			_preferencesDialog->getOdomQualityWarnThr() &&
-			quality < _preferencesDialog->getOdomQualityWarnThr())
+			info.inliers < _preferencesDialog->getOdomQualityWarnThr())
 	{
-		UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr());
+		UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
 		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow));
 	}
@@ -621,22 +622,42 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
 		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
 	}
-	if(quality >= 0)
+	if(info.inliers >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)quality);
+		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
 	}
-	if(time > 0)
+	if(info.matches >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
 	}
-	if(features >=0)
+	if(info.variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)features);
+		_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
 	}
-	if(localMapSize >=0)
+	if(info.variance >= 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
+		_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
 	}
+	if(info.time > 0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f);
+	}
+	if(info.features >=0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
+	}
+	if(info.localMapSize >=0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
+	}
+	float x,y,z, roll,pitch,yaw;
+	pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
+	_ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y);
+	_ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z);
+	_ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI);
+	_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
+	_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
 
 	if(_ui->dockWidget_cloudViewer->isVisible())
 	{
@@ -677,11 +698,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 			}
 
 			// 2d cloud
-			if(!data.depth2d().empty() &&
+			if(!data.laserScan().empty() &&
 				_preferencesDialog->isScansShown(1))
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-				cloud = util3d::depth2DToPointCloud(data.depth2d());
+				cloud = util3d::laserScanToPointCloud(data.laserScan());
 				cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
 				if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
 				{
@@ -1039,7 +1060,7 @@ void MainWindow::updateMapCloud(
 
 			if(!_ui->actionView_scans->isEnabled() &&
 				_cachedSignatures.size() &&
-				!(--_cachedSignatures.end())->getDepth2DCompressed().empty())
+				!(--_cachedSignatures.end())->getLaserScanCompressed().empty())
 			{
 				_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1158,7 +1179,7 @@ void MainWindow::updateMapCloud(
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
-					if(!jter->getDepth2DCompressed().empty())
+					if(!jter->getLaserScanCompressed().empty())
 					{
 						this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					}
@@ -1441,13 +1462,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		return;
 	}
 
-	if(!iter->getDepth2DCompressed().empty())
+	if(!iter->getLaserScanCompressed().empty())
 	{
 		cv::Mat depth2D;
 		iter->uncompressData(0, 0, &depth2D);
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-		cloud = util3d::depth2DToPointCloud(depth2D);
+		cloud = util3d::laserScanToPointCloud(depth2D);
 		QColor color = Qt::red;
 		if(mapId >= 0)
 		{
@@ -2342,6 +2363,7 @@ void MainWindow::startDetection()
 			}
 			return;
 		}
+
 		if(_odomThread)
 		{
 			UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
@@ -2746,9 +2768,11 @@ void MainWindow::postProcessing()
 	_initProgressDialog->show();
 
 	ParametersMap parameters = _preferencesDialog->getAllParameters();
-	int toroIterations = 100;
+	int toroIterations = Parameters::defaultRGBDToroIterations();
-	bool toroOptimizeFromGraphEnd = false;
+	bool ignoreVariance =  Parameters::defaultRGBDToroIgnoreVariance();
+	bool toroOptimizeFromGraphEnd =  Parameters::defaultRGBDOptimizeFromGraphEnd();
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
+	Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), ignoreVariance);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
 
 	int loopClosuresAdded = 0;
@@ -2819,7 +2843,8 @@ void MainWindow::postProcessing()
 
 						Transform transform;
 						std::string rejectedMsg;
-						int inliers;
+						int inliers = -1;
+						double variance = -1.0;
 						if(reextractFeatures)
 						{
 							memory.init("", true); // clear previously added signatures
@@ -2846,7 +2871,7 @@ void MainWindow::postProcessing()
 									memory.update(dataTo);
 								}
 
-								transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers);
+								transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance);
 							}
 							else
 							{
@@ -2855,14 +2880,14 @@ void MainWindow::postProcessing()
 						}
 						else
 						{
-							transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers);
+							transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance);
 						}
 						if(!transform.isNull())
 						{
 							UINFO("Added new loop closure between %d and %d.", from, to);
 							addedLinks.insert(from);
 							addedLinks.insert(to);
-							_currentLinksMap.insert(std::make_pair(from, Link(from, to, transform, Link::kUserClosure)));
+							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance)));
 							++loopClosuresAdded;
 							_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 						}
@@ -2882,7 +2907,7 @@ void MainWindow::postProcessing()
 						.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 				std::map<int, rtabmap::Transform> optimizedPoses;
 				std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-				util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
+				util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 				_currentPosesMap = optimizedPoses;
 				_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
 			}
@@ -2903,14 +2928,14 @@ void MainWindow::postProcessing()
 		float maxDepth=2.0f;
 		float voxelSize=0.01f;
 		int samples = 0;
-		float minFitness = 1.0f;
 		float maxCorrespondences = 0.05f;
+		float correspondenceRatio = 0.7f;
 		float icpIterations = 30;
 		Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
 		Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
 		Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
 		Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
-		Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), minFitness);
+		Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
 		Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
 		Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
 		bool pointToPlane = false;
@@ -2974,7 +2999,8 @@ void MainWindow::postProcessing()
 							iter->second.transform() * signatureTo.getLocalTransform());
 
 					bool hasConverged = false;
-					double fitness = -1;
+					double variance = -1;
+					int correspondences = 0;
 					Transform transform;
 					if(pointToPlane)
 					{
@@ -2997,8 +3023,9 @@ void MainWindow::postProcessing()
 								cloudANormals,
 								maxCorrespondences,
 								icpIterations,
-								hasConverged,
+								&hasConverged,
-								fitness);
+								&variance,
+								&correspondences);
 					}
 					else
 					{
@@ -3006,18 +3033,22 @@ void MainWindow::postProcessing()
 								cloudA,
 								maxCorrespondences,
 								icpIterations,
-								hasConverged,
+								&hasConverged,
-								fitness);
+								&variance,
+								&correspondences);
 					}
 
-					if(hasConverged && !transform.isNull() && fitness>=0.0f && fitness <= minFitness)
+					float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
+
+					if(!transform.isNull() && hasConverged &&
+					   correspondencesRatio >= correspondenceRatio)
 					{
-						Link newLink(from, to, transform*iter->second.transform(), iter->second.type());
+						Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance);
 						iter->second = newLink;
 					}
 					else
 					{
-						UWARN("Cannot refine link %d->%d (converged=%s fitness=%f)", from, to, hasConverged?"true":"false", fitness);
+						UWARN("Cannot refine link %d->%d (converged=%s variance=%f)", from, to, hasConverged?"true":"false", variance);
 					}
 				}
 			}
@@ -3029,7 +3060,7 @@ void MainWindow::postProcessing()
 			.arg(odomPoses.size()).arg(_currentLinksMap.size()));
 	std::map<int, rtabmap::Transform> optimizedPoses;
 	std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
-	util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
+	util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
 	_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
 	_initProgressDialog->incrementStep();
 
@@ -4698,7 +4729,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->statusbar->showMessage(tr("Paused..."));
 			_ui->actionDump_the_memory->setEnabled(true);
 			_ui->actionDump_the_prediction_matrix->setEnabled(true);
-			_ui->actionDelete_memory->setEnabled(true);
+			_ui->actionDelete_memory->setEnabled(false);
 			_ui->actionGenerate_map->setEnabled(true);
 			_ui->actionGenerate_local_map->setEnabled(true);
 			_ui->actionGenerate_TORO_graph_graph->setEnabled(true);

guilib/src/OdometryViewer.cpp

@@ -187,7 +187,7 @@ void OdometryViewer::handleEvent(UEvent * event)
 			{
 				data_.back() = odomEvent->data();
 			}
-			dataQuality_ = odomEvent->quality();
+			dataQuality_ = odomEvent->info().inliers;
 			dataMutex_.unlock();
 			if(empty)
 			{

guilib/src/PreferencesDialog.cpp

@@ -440,6 +440,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
 	_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
+	_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
 	_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
@@ -469,13 +470,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpSamples->setObjectName(Parameters::kLccIcp3Samples().c_str());
 	_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str());
 	_ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str());
-	_ui->loopClosure_icpMaxFitness->setObjectName(Parameters::kLccIcp3MaxFitness().c_str());
+	_ui->loopClosure_icpRatio->setObjectName(Parameters::kLccIcp3CorrespondenceRatio().c_str());
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str());
 	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str());
 
 	_ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str());
 	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
-	_ui->loopClosure_icp2MaxFitness->setObjectName(Parameters::kLccIcp2MaxFitness().c_str());
 	_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
 	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
 

guilib/src/ui/DatabaseViewer.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1187</width>
-    <height>851</height>
+    <height>862</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -354,13 +354,6 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
-        <widget class="QLabel" name="label_constraint">
-         <property name="text">
-          <string/>
-         </property>
-        </widget>
-       </item>
        <item row="2" column="0">
         <widget class="QLabel" name="label_16">
          <property name="text">
@@ -368,17 +361,24 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="2" column="1">
-        <widget class="QLabel" name="label_18">
+        <widget class="QLabel" name="label_constraint">
          <property name="text">
-          <string>Fitness</string>
+          <string/>
+         </property>
+        </widget>
+       </item>
+       <item row="3" column="0">
+        <widget class="QCheckBox" name="checkBox_showOptimized">
+         <property name="text">
+          <string>Optimized</string>
          </property>
         </widget>
        </item>
        <item row="3" column="1">
-        <widget class="QLabel" name="label_fitness">
+        <widget class="QLabel" name="label_constraint_opt">
          <property name="text">
-          <string>-</string>
+          <string/>
          </property>
         </widget>
        </item>
@@ -511,14 +511,14 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
+       <item row="3" column="0">
         <widget class="QLabel" name="label_8">
          <property name="text">
           <string>Optimize from:</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
+       <item row="3" column="1">
         <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
        </item>
        <item row="1" column="1">
@@ -538,20 +538,37 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="4" column="0">
         <widget class="QLabel" name="label_10">
          <property name="text">
           <string>Total path length (m):</string>
          </property>
         </widget>
        </item>
-       <item row="3" column="1">
+       <item row="4" column="1">
         <widget class="QLabel" name="label_pathLength">
          <property name="text">
           <string/>
          </property>
         </widget>
        </item>
+       <item row="2" column="0">
+        <widget class="QLabel" name="label_34">
+         <property name="text">
+          <string>Ignore covariance:</string>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="1">
+        <widget class="QCheckBox" name="checkBox_ignoreCovariance">
+         <property name="text">
+          <string/>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
     </layout>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-305</y>
             <width>744</width>
-            <height>1074</height>
+            <height>1252</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5174,7 +5174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="1">
+                       <item row="2" column="1">
                         <widget class="QLabel" name="label_151">
                          <property name="text">
                           <string>Optimize graph from the newest node.
@@ -5187,13 +5187,30 @@ Warning when set to false: when some nodes are transferred, the first referentia
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="0">
+                       <item row="2" column="0">
                         <widget class="QCheckBox" name="globalDetection_optimizeFromGraphEnd">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_141">
+                         <property name="text">
+                          <string>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.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5209,7 +5226,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
                        <item>
                         <widget class="QLabel" name="label_54">
                          <property name="text">
-                          <string>Activate local detection over all locations in STM. The Bayes filter is not used here, so it may results in more false detections. The same 2-steps technique as for global loop closure constraints is used here.</string>
+                          <string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. The same 2-steps technique as for global loop closure constraints is used here. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5837,19 +5854,25 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </widget>
                        </item>
                        <item row="6" column="0">
-                        <widget class="QDoubleSpinBox" name="loopClosure_icpMaxFitness">
+                        <widget class="QDoubleSpinBox" name="loopClosure_icpRatio">
                          <property name="minimum">
+                          <double>0.000000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
                           <double>0.010000000000000</double>
                          </property>
                          <property name="value">
-                          <double>30.000000000000000</double>
+                          <double>0.700000000000000</double>
                          </property>
                         </widget>
                        </item>
                        <item row="6" column="1">
                         <widget class="QLabel" name="label_133">
                          <property name="text">
-                          <string>Maximum fitness to accept the computed transform.</string>
+                          <string>Ratio of matching correspondences to accept the transform.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5955,26 +5978,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </widget>
                        </item>
                        <item row="2" column="0">
-                        <widget class="QDoubleSpinBox" name="loopClosure_icp2MaxFitness">
-                         <property name="minimum">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>30.000000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_141">
-                         <property name="text">
-                          <string>Maximum fitness to accept the computed transform.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="0">
                         <widget class="QDoubleSpinBox" name="loopClosure_icp2Ratio">
                          <property name="minimum">
                           <double>0.000000000000000</double>
@@ -5986,11 +5989,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                           <double>0.010000000000000</double>
                          </property>
                          <property name="value">
-                          <double>0.900000000000000</double>
+                          <double>0.700000000000000</double>
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                       <item row="2" column="1">
                         <widget class="QLabel" name="label_148">
                          <property name="text">
                           <string>Ratio of matching correspondences to accept the transform.</string>
@@ -6000,17 +6003,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="1">
+                       <item row="3" column="0">
-                        <widget class="QLabel" name="label_150">
-                         <property name="text">
-                          <string>Voxel size. Set to 0 to disable voxel filtering.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
                         <widget class="QDoubleSpinBox" name="loopClosure_icp2Voxel">
                          <property name="suffix">
                           <string> m</string>
@@ -6032,6 +6025,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_150">
+                         <property name="text">
+                          <string>Voxel size. Set to 0 to disable voxel filtering.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>

guilib/src/utilite/UPlot.cpp

@@ -19,6 +19,7 @@
 
 #include "UPlot.h"
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
 
 #include <QtGui/QGraphicsScene>
 #include <QtGui/QGraphicsView>
@@ -1341,6 +1342,8 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) :
 	_aResetText = new QAction(tr("Reset text..."), this);
 	_aChangeColor = new QAction(tr("Change color..."), this);
 	_aCopyToClipboard = new QAction(tr("Copy curve data to the clipboard"), this);
+	_aShowStdDev = new QAction(tr("Show std deviation"), this);
+	_aShowStdDev->setCheckable(true);
 	_aMoveUp = new QAction(tr("Move up"), this);
 	_aMoveDown = new QAction(tr("Move down"), this);
 	_aRemoveCurve = new QAction(tr("Remove this curve"), this);
@@ -1349,6 +1352,7 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) :
 	_menu->addAction(_aResetText);
 	_menu->addAction(_aChangeColor);
 	_menu->addAction(_aCopyToClipboard);
+	_menu->addAction(_aShowStdDev);
 	_menu->addSeparator();
 	_menu->addAction(_aMoveUp);
 	_menu->addAction(_aMoveDown);
@@ -1416,6 +1420,20 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event)
 			clipboard->setText((textX+"\n")+textY);
 		}
 	}
+	else if(action == _aShowStdDev)
+	{
+		if(_aShowStdDev->isChecked())
+		{
+			connect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev()));
+		}
+		else
+		{
+			disconnect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev()));
+			QString nameSpaced = _curve->name();
+			nameSpaced.replace('_', ' ');
+			this->setText(nameSpaced);
+		}
+	}
 	else if(action == _aRemoveCurve)
 	{
 		emit legendItemRemoved(_curve);
@@ -1442,6 +1460,17 @@ QPixmap UPlotLegendItem::createSymbol(const QPen & pen, const QBrush & brush)
 	return pixmap;
 }
 
+void UPlotLegendItem::updateStdDev()
+{
+	QVector<float> x, y;
+	_curve->getData(x, y);
+	float stdDev = std::sqrt(uVariance(y.data(), y.size()));
+	QString nameSpaced = _curve->name();
+	nameSpaced.replace('_', ' ');
+	nameSpaced += QString(" (%1=%2)").arg(QChar(0xc3, 0x03)).arg(stdDev);
+	this->setText(nameSpaced);
+}
+
 
 
 

guilib/src/utilite/UPlot.h

@@ -356,6 +356,9 @@ signals:
 	void moveUpRequest(UPlotLegendItem *);
 	void moveDownRequest(UPlotLegendItem *);
 
+private slots:
+	void updateStdDev();
+
 protected:
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 
@@ -366,6 +369,7 @@ private:
 	QAction * _aResetText;
 	QAction * _aChangeColor;
 	QAction * _aCopyToClipboard;
+	QAction * _aShowStdDev;
 	QAction * _aRemoveCurve;
 	QAction * _aMoveUp;
 	QAction * _aMoveDown;

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.7.3</version>
+  <version>0.8.0</version>
   <description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/DataRecorder/main.cpp

@@ -46,7 +46,8 @@ void showUsage()
 			"  -debug                   Set debug level for the logger.\n"
 			"  -rate #.#                Input rate Hz (default 0=inf)\n"
 			"  -openni                  Use openni camera instead of the usb camera.\n"
-			"  -openni2                 Use openni2 camera instead of the usb camera.\n");
+			"  -openni2                 Use openni2 camera instead of the usb camera.\n"
+			"  -freenect                Use freenect camera instead of the usb camera.\n");
 	exit(1);
 }
 
@@ -76,6 +77,7 @@ int main (int argc, char * argv[])
 	bool show = true;
 	bool openni = false;
 	bool openni2 = false;
+	bool freenect = false;
 	float rate = 0.0f;
 
 	if(argc < 2)
@@ -121,6 +123,11 @@ int main (int argc, char * argv[])
 			openni2 = true;
 			continue;
 		}
+		if(strcmp(argv[i], "-freenect") == 0)
+		{
+			freenect = true;
+			continue;
+		}
 
 		printf("Unrecognized option : %s\n", argv[i]);
 		showUsage();
@@ -135,7 +142,33 @@ int main (int argc, char * argv[])
 
 	UINFO("Output = %s", fileName.toStdString().c_str());
 	UINFO("Show = %s", show?"true":"false");
-	UINFO("Openni = %s", openni?"true":"false");
+	if(openni)
+	{
+		UINFO("Openni = true");
+		if(!CameraOpenni::available())
+		{
+			UERROR("Openni is not available. Please select another driver.");
+			return -1;
+		}
+	}
+	else if(openni2)
+	{
+		UINFO("Openni2 = true");
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Openni2 is not available. Please select another driver.");
+			return -1;
+		}
+	}
+	else if(freenect)
+	{
+		UINFO("Freenect = true");
+		if(!CameraFreenect::available())
+		{
+			UERROR("Freenect is not available. Please select another driver.");
+			return -1;
+		}
+	}
 	UINFO("Rate =%f Hz", rate);
 
 	app = new QApplication(argc, argv);
@@ -154,6 +187,10 @@ int main (int argc, char * argv[])
 	{
 		cam = new rtabmap::CameraThread(new rtabmap::CameraOpenni("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
 	}
+	else if(freenect)
+	{
+		cam = new rtabmap::CameraThread(new rtabmap::CameraFreenect(0, rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
+	}
 	else
 	{
 		cam = new rtabmap::CameraThread(new rtabmap::CameraVideo(0, rate));

tools/OdometryViewer/main.cpp

@@ -73,7 +73,7 @@ void showUsage()
 			"  -d #                      ICP decimation (default 4)\n"
 			"  -v #                      ICP voxel size (default 0.005)\n"
 			"  -s #                      ICP samples (default 0, not used if voxel is set.)\n"
-			"  -f #.#                    ICP fitness (default 0.01)\n"
+			"  -cr #.#                   ICP correspondence ratio (default 0.7)\n"
 			"  -p2p                      ICP point to point (default point to plane)"
 			"\n"
 			"  -debug                    Log debug messages\n"
@@ -114,7 +114,7 @@ int main (int argc, char * argv[])
 	int decimation = 4;
 	float voxel = 0.005;
 	int samples = 10000;
-	float fitness = 0.01f;
+	float ratio = 0.7f;
 	int maxClouds = 10;
 	int briefBytes = 32;
 	int fastThr = 30;
@@ -466,13 +466,13 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-f") == 0)
+		if(strcmp(argv[i], "-cr") == 0)
 		{
 			++i;
 			if(i < argc)
 			{
-				fitness = std::atof(argv[i]);
+				ratio = std::atof(argv[i]);
-				if(fitness < 0.0f)
+				if(ratio < 0.0f)
 				{
 					showUsage();
 				}
@@ -494,7 +494,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				localHistory = std::atoi(argv[i]);
-				if(fitness <= 0)
+				if(localHistory < 0)
 				{
 					showUsage();
 				}
@@ -735,10 +735,10 @@ int main (int argc, char * argv[])
 		UINFO("Cloud decimation =        %d", decimation);
 		UINFO("Cloud voxel size =        %f", voxel);
 		UINFO("Cloud samples =           %d", samples);
-		UINFO("Cloud fitness =           %f", fitness);
+		UINFO("Cloud correspondence ratio = %f", ratio);
 		UINFO("Cloud point to plane =    %s", p2p?"false":"true");
 
-		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness, !p2p);
+		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
 	}
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);

utilite/include/rtabmap/utilite/UMath.h

@@ -460,15 +460,15 @@ inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
 }
 
 /**
- * Compute the standard deviation of an array.
+ * Compute the variance of an array.
  * @param v the array
  * @param size the size of the array
  * @param meanV the mean of the array
- * @return the std dev
+ * @return the variance
  * @see mean()
  */
 template<class T>
-inline T uStdDev(const T * v, unsigned int size, T meanV)
+inline T uVariance(const T * v, unsigned int size, T meanV)
 {
 	T buf = 0;
 	if(v && size>1)
@@ -478,20 +478,20 @@ inline T uStdDev(const T * v, unsigned int size, T meanV)
 		{
 			sum += (v[i]-meanV)*(v[i]-meanV);
 		}
-		buf = sqrt(sum/(size-1));
+		buf = sum/(size-1);
 	}
 	return buf;
 }
 
 /**
- * Get the standard deviation of a list. Provided for convenience.
+ * Get the variance of a list. Provided for convenience.
  * @param list the list
  * @param m the mean of the list
- * @return the std dev
+ * @return the variance
  * @see mean()
  */
 template<class T>
-inline T uStdDev(const std::list<T> & list, const T & m)
+inline T uVariance(const std::list<T> & list, const T & m)
 {
 	T buf = 0;
 	if(list.size()>1)
@@ -501,35 +501,35 @@ inline T uStdDev(const std::list<T> & list, const T & m)
 		{
 			sum += (*i-m)*(*i-m);
 		}
-		buf = sqrt(sum/(list.size()-1));
+		buf = sum/(list.size()-1);
 	}
 	return buf;
 }
 
 /**
- * Compute the standard deviation of an array.
+ * Compute the variance of an array.
  * @param v the array
  * @param size the size of the array
- * @return the std dev
+ * @return the variance
  */
 template<class T>
-inline T uStdDev(const T * v, unsigned int size)
+inline T uVariance(const T * v, unsigned int size)
 {
 	T m = uMean(v, size);
-	return uStdDev(v, size, m);
+	return uVariance(v, size, m);
 }
 
 /**
- * Get the standard deviation of a vector. Provided for convenience.
+ * Get the variance of a vector. Provided for convenience.
  * @param v the vector
  * @param m the mean of the vector
- * @return the std dev
+ * @return the variance
  * @see mean()
  */
 template<class T>
-inline T uStdDev(const std::vector<T> & v, const T & m)
+inline T uVariance(const std::vector<T> & v, const T & m)
 {
-	return uStdDev(v.data(), v.size(), m);
+	return uVariance(v.data(), v.size(), m);
 }
 
 /**