New classes: Link and GraphViewer (TORO graph visualization and laser scans occupancy grid)

New parameter: RGBD/ToroIterations=100
Rtabmap: added getGraph() method to get TORO poses/link constraints
Fixed ICP correspondences ratio over 1
CloudViewer: added frustum culling and camera view up z lock in render(), moving camera using arrow keys, Menu options: Camera far plane clipping, background color
DatabaseViewer: added 3D Map view and Graph view
MainWindow: Moved main widget (loop closure image status) to a dock widget, added Graph view.


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1075 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Link.h

@@ -0,0 +1,42 @@
+/*
+ * Link.h
+ *
+ *  Created on: 2014-01-29
+ *      Author: mathieu
+ */
+
+#ifndef LINK_H_
+#define LINK_H_
+
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap {
+
+class Link
+{
+public:
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure};
+	Link(int from, int to, const Transform & transform, Type type) :
+		from_(from),
+		to_(to),
+		transform_(transform),
+		type_(type)
+	{
+	}
+
+	int from() const {return from_;}
+	int to() const {return to_;}
+	const Transform & transform() const {return transform_;}
+	Type type() const {return type_;}
+
+private:
+	int from_;
+	int to_;
+	Transform transform_;
+	Type type_;
+};
+
+}
+
+
+#endif /* LINK_H_ */

corelib/include/rtabmap/core/Memory.h

@@ -25,6 +25,7 @@
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Image.h"
+#include "rtabmap/core/Link.h"
 #include <typeinfo>
 #include <list>
 #include <map>
@@ -144,7 +145,7 @@ public:
 	void getMetricConstraints(
 			const std::vector<int> & ids,
 			std::map<int, Transform> & poses,
-			std::multimap<int, std::pair<int, Transform> > & links,
+			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 	Transform computeVisualTransform(int oldId, int newId) const;
 	Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;

corelib/include/rtabmap/core/Parameters.h

@@ -204,6 +204,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ScanMatchingSize,  int, 0, 		"Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
 	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	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, ToroIterations,    int, 100,    "TORO graph optimization iterations")
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, true, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -25,6 +25,7 @@
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Image.h"
 #include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Link.h"
 
 #include <opencv2/core/core.hpp>
 #include <list>
@@ -101,8 +102,13 @@ public:
 			std::map<int, float> & depthConstants,
 			std::map<int, Transform> & localTransforms,
 			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & constraints,
 			bool optimized,
 			bool full) const;
+	void getGraph(std::map<int, Transform> & poses,
+			std::multimap<int, Link> & constraints,
+			bool optimized,
+			bool full);
 
 	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
@@ -113,7 +119,7 @@ private:
 	void optimizeCurrentMap(int id,
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
-			std::multimap<int, std::pair<int, Transform> > * constraints = 0) const;
+			std::multimap<int, Link> * constraints = 0) const;
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -142,7 +148,7 @@ private:
 	float _localDetectRadius;
 	float _localDetectMaxNeighbors;
 	int _localDetectMaxDiffID;
-	bool _icpEnabled;
+	int _toroIterations;
 	std::string _databasePath;
 
 	int _lcHypothesisId;
@@ -167,6 +173,7 @@ private:
 	std::string _wDir;
 
 	std::map<int, Transform> _optimizedPoses;
+	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection; // for localization mode
 	Transform _mapTransform; // for localization mode
 };

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -58,8 +58,10 @@ public:
 			kCmdGenerateTOROGraphFull, // params: path, optimized
 			kCmdDeleteMemory, // params: path [optional]
 			kCmdCleanDataBuffer,
-			kCmdPublish3DMap,
-			kCmdPublish3DMapFull,
+			kCmdPublish3DMap, // params: optimized
+			kCmdPublish3DMapFull, // params: optimized
+			kCmdPublishGraphFull, // params: optimized
+			kCmdPublishGraph, // params: optimized
 			kCmdTriggerNewMap,
 			kCmdPause};
 public:
@@ -147,14 +149,16 @@ public:
 			const std::map<int, std::vector<unsigned char> > & depths2d,
 			const std::map<int, float> & depthConstants,
 			const std::map<int, Transform> & localTransforms,
-			const std::map<int, Transform> & poses) :
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints) :
 		UEvent(0),
 		_images(images),
 		_depths(depths),
 		_depths2d(depths2d),
 		_depthConstants(depthConstants),
 		_localTransforms(localTransforms),
-		_poses(poses)
+		_poses(poses),
+		_constraints(constraints)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -165,6 +169,7 @@ public:
 	const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
 	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
+	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -175,6 +180,7 @@ private:
 	std::map<int, float> _depthConstants;
 	std::map<int, Transform> _localTransforms;
 	std::map<int, Transform> _poses;
+	std::multimap<int, Link> _constraints;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -59,6 +59,9 @@ public:
 		kStateCleanDataBuffer,
 		kStatePublishingMap,
 		kStatePublishingMapFull,
+		kStatePublishingGraph,
+		kStatePublishingGraphFull,
+		Graph,
 		kStateTriggeringMap
 	};
 
@@ -80,6 +83,7 @@ private:
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
 	void publishMap(bool optimized, bool full) const;
+	void publishGraph(bool optimized, bool full) const;
 
 private:
 	UMutex _stateMutex;

corelib/include/rtabmap/core/Statistics.h

@@ -27,7 +27,7 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <list>
 #include <vector>
-#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
 
 namespace rtabmap {
 
@@ -56,6 +56,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_closure_id,);
+	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
 	RTABMAP_STATS(LocalLoop, Space_neighbors,);
 	RTABMAP_STATS(LocalLoop, Space_diff_id,);
 
@@ -117,6 +118,7 @@ public:
 	void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
+	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
 	void setCurrentPose(const Transform & pose) {_currentPose = pose;}
 	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
 	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
@@ -141,6 +143,7 @@ public:
 	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
+	const std::multimap<int, Link> & constraints() const {return _constraints;}
 	const Transform & currentPose() const {return _currentPose;}
 	const Transform & mapCorrection() const {return _mapCorrection;}
 	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
@@ -171,6 +174,7 @@ private:
 	std::map<int, Transform> _localTransforms;
 
 	std::map<int, Transform> _poses;
+	std::multimap<int, Link> _constraints;
 	Transform _currentPose;
 	Transform _mapCorrection;
 	Transform _loopClosureTransform;

corelib/include/rtabmap/core/util3d.h

@@ -12,7 +12,7 @@
 #include <list>
 #include <string>
 
-#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
 #include <rtabmap/utilite/UThread.h>
 #include <pcl/common/eigen.h>
 #include <pcl/point_types.h>
@@ -342,20 +342,32 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud<pcl::PointXYZ
 
 void RTABMAP_EXP optimizeTOROGraph(
 		const std::map<int, Transform> & poses,
-		const std::multimap<int, std::pair<int, Transform> > & edgeConstraints,
-		int toroIterations,
+		const std::multimap<int, Link> & edgeConstraints,
 		std::map<int, Transform> & optimizedPoses,
-		Transform & mapCorrection);
+		Transform & mapCorrection,
+		int toroIterations = 100,
+		bool toroInitialGuess = true,
+		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
 
 bool RTABMAP_EXP saveTOROGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,
-		const std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
+		const std::multimap<int, Link> & edgeConstraints);
 
 bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
 		std::map<int, Transform> & poses,
 		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
 
+cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
+		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
+		float delta,
+		float & xMin,
+		float & yMin);
+
+void RTABMAP_EXP rayTrace(const cv::Point2i & start,
+		const cv::Point2i & end,
+		cv::Mat & grid);
+
 } // namespace util3d
 } // namespace rtabmap