Increased version to 0.6.2

Memory management tested on RGBD SLAM
Fixed some crashes
New parameter RGBD/LocalLoopDetectionMaxDiffID
New parameter LccIcp2/VoxelSize
Updated parameter LccIcp/Type (added "No ICP" type)
Updated parameter kLccBowMinInliers (inliers minimum reduced to 1)
TORO optimization: The graph root is explicitly defined as the last node added (fixed a TORO crash when a root could not be found)
TORO optimization: only one constraint between neighbors and loop closures
TORO optimization: added initial tree guess
Map correction/Map transform: now only used in localization mode (map correction is always identity on mapping mode)
New statistics: local loop space diff, last loop closure parent and child ids
Database: Fixed empty signatures with no poses loaded
ICP: fixed transform multiplication order
Dump memory: added 3D words
DatabaseViewer: added Export option (added ExportDialog object)
DataRecorder: Option recording in RAM or Hard drive
GUI: added a dock widget (MapVisibilityWidget) to change visibility of nodes in the 3D map
GUI: fixed progress bar step count when generating the map
Menu option: generate TORO graph (full/current map, optimized/not optimized)
Menu option: download map (full/current map, optimized/not optimized)
Preferences: added Reset all settings button
Preferences: A QGroupBox can be a boolean parameter

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

corelib/include/rtabmap/core/Memory.h

@@ -71,7 +71,7 @@ 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 addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
 	void updateNeighborLink(int fromId, int toId, const Transform & transform);
 	std::map<int, int> getNeighborsId(int signatureId,
 			unsigned int margin,
@@ -87,7 +87,6 @@ public:
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
 	void getPose(int locationId,
-			int targetMapId,
 			Transform & pose,
 			bool lookInDatabase = false) const;
 	std::map<int, Transform> getNeighborLinks(int signatureId,
@@ -119,12 +118,14 @@ public:
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
 	bool isIDsGenerated() const {return _generateIds;}
+	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
+	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
 
 	void setRoi(const std::string & roi);
 
 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
-	virtual void dumpSignatures(const char * fileNameSign) const;
+	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
 
 	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
@@ -142,14 +143,13 @@ public:
 
 	void getMetricConstraints(
 			const std::vector<int> & ids,
-			int targetMapId,
 			std::map<int, Transform> & poses,
 			std::multimap<int, std::pair<int, Transform> > & links,
 			bool lookInDatabase = false);
 	Transform computeVisualTransform(int oldId, int newId) const;
 	Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
@@ -201,7 +201,8 @@ private:
 	int _idCount;
 	int _idMapCount;
 	Signature * _lastSignature;
-	int _lastLoopClosureId;
+	int _lastGlobalLoopClosureParentId;
+	int _lastGlobalLoopClosureChildId;
 	bool _memoryChanged; // False by default, become true when Memory::update() is called.
 	int _signaturesAdded;
 
@@ -220,7 +221,6 @@ private:
 	std::vector<float> _roiRatios; // size 4
 
 	// RGBD-SLAM stuff
-	int _icpType;
 	int _bowMinInliers;
 	float _bowInlierDistance;
 	int _bowIterations;
@@ -236,6 +236,7 @@ private:
 	int _icp2MaxIterations;
 	float _icp2MaxFitness;
 	float _icp2CorrespondenceRatio;
+	float _icp2VoxelSize;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -210,6 +210,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=BOW 1=Binary.");
@@ -235,8 +236,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomICP, MaxFitness,               float, 0.01, 	"");
 
 	// Loop closure constraint
-	RTABMAP_PARAM(LccIcp, Enabled,         bool, false, 	"Enable ICP");
-	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=ICP 3D, 1=ICP 2D");
+	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 0=ICP 3D, 1=ICP 2D");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.01, 	"Maximum distance for visual word correspondences.");
@@ -255,7 +255,7 @@ class RTABMAP_EXP Parameters
 	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.");
 
 public:
 	virtual ~Parameters();

corelib/include/rtabmap/core/Rtabmap.h

@@ -75,7 +75,6 @@ public:
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
 	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
-	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
@@ -87,6 +86,7 @@ public:
 
 	void triggerNewMap();
 	void generateGraph(const std::string & path, int id=0, int margin=5);
+	void generateTOROGraph(const std::string & path, bool optimized, bool full);
 	void resetMemory(bool dbOverwritten = false);
 	void dumpPrediction() const;
 	void dumpData() const;
@@ -101,15 +101,20 @@ public:
 			std::map<int, float> & depthConstants,
 			std::map<int, Transform> & localTransforms,
 			std::map<int, Transform> & poses,
-			Transform & mapCorrection) const;
+			bool optimized,
+			bool full) const;
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int & nearestId) const;
+	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
 
 private:
-	void optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const;
+	void optimizeCurrentMap(int id,
+			bool lookInDatabase,
+			std::map<int, Transform> & optimizedPoses,
+			std::multimap<int, std::pair<int, Transform> > * constraints = 0) const;
+
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
 
@@ -130,11 +135,13 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	int _globalLoopClosureIcpType;
 	int _scanMatchingSize;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localDetectRadius;
 	float _localDetectMaxNeighbors;
+	int _localDetectMaxDiffID;
 	bool _icpEnabled;
 	std::string _databasePath;
 
@@ -160,8 +167,8 @@ private:
 	std::string _wDir;
 
 	std::map<int, Transform> _optimizedPoses;
-	Transform _mapCorrection;
-	Transform _mapTransform;
+	Transform _mapCorrection; // for localization mode
+	Transform _mapTransform; // for localization mode
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -52,11 +52,14 @@ public:
 			kCmdResetMemory,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
-			kCmdGenerateGraph,
-			kCmdGenerateLocalGraph,
-			kCmdDeleteMemory,
+			kCmdGenerateGraph, // params: path
+			kCmdGenerateLocalGraph, // params: path, id, margin
+			kCmdGenerateTOROGraph, // params: path, optimized
+			kCmdGenerateTOROGraphFull, // params: path, optimized
+			kCmdDeleteMemory, // params: path [optional]
 			kCmdCleanDataBuffer,
 			kCmdPublish3DMap,
+			kCmdPublish3DMapFull,
 			kCmdTriggerNewMap,
 			kCmdPause};
 public:
@@ -75,6 +78,11 @@ public:
 			_cmd(cmd),
 			_strValue(value),
 			_intValue(0){}
+	RtabmapEventCmd(Cmd cmd, const std::string & strValue, int intValue) :
+			UEvent(0),
+			_cmd(cmd),
+			_strValue(strValue),
+			_intValue(intValue){}
 
 	virtual ~RtabmapEventCmd() {}
 	Cmd getCmd() const {return _cmd;}
@@ -139,16 +147,14 @@ 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 Transform & mapCorrection) :
+			const std::map<int, Transform> & poses) :
 		UEvent(0),
 		_images(images),
 		_depths(depths),
 		_depths2d(depths2d),
 		_depthConstants(depthConstants),
 		_localTransforms(localTransforms),
-		_poses(poses),
-		_mapCorrection(mapCorrection)
+		_poses(poses)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -159,7 +165,6 @@ 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 Transform & getMapCorrection() const {return _mapCorrection;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -170,7 +175,6 @@ private:
 	std::map<int, float> _depthConstants;
 	std::map<int, Transform> _localTransforms;
 	std::map<int, Transform> _poses;
-	Transform _mapCorrection;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -53,9 +53,12 @@ public:
 		kStateDumpingPrediction,
 		kStateGeneratingGraph,
 		kStateGeneratingLocalGraph,
+		kStateGeneratingTOROGraph,
+		kStateGeneratingTOROGraphFull,
 		kStateDeletingMemory,
 		kStateCleanDataBuffer,
 		kStatePublishingMap,
+		kStatePublishingMapFull,
 		kStateTriggeringMap
 	};
 
@@ -76,7 +79,7 @@ private:
 	void getImage(Image & image);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
-	void publishMap() const;
+	void publishMap(bool optimized, bool full) const;
 
 private:
 	UMutex _stateMutex;

corelib/include/rtabmap/core/Statistics.h

@@ -50,11 +50,14 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
+	RTABMAP_STATS(Loop, Last_loop_closure_child,);
 
-	RTABMAP_STATS(LocalLoop, Scan_matching_success,);
+	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_closure_id,);
 	RTABMAP_STATS(LocalLoop, Space_neighbors,);
+	RTABMAP_STATS(LocalLoop, Space_diff_id,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
@@ -63,7 +66,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
-	RTABMAP_STATS(Memory, Last_loop_closure,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);