Integration of the path planner into rtabmap's memory management to actually retrieve nodes on the planned path on the graph.

Added Rtabmap::clearPath(), Rtabmap::computePath(), Rtabmap::getPathGoalId() and Rtabmap::updateGoalIndex() methods.
Added virtual links when path retrieval is activated (virtual links are not saved to database and removed when the path goal is reached).
New parameters: RGBD/GoalReachedRadius and RGBD/MaxAnticipatedNodes
Refactored how loop closure hypotheses are selected (fixed ratio between consecutive hypotheses, not the last loop closure).
Updated getNodesInRadius() to use all filtered scans.
GUI: window can be saved/loaded maximized
GUI: saving Graph View parameters to config.ini

corelib/include/rtabmap/core/Link.h

@@ -35,7 +35,7 @@ namespace rtabmap {
 class Link
 {
 public:
-	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
 	Link() :
 		from_(0),
 		to_(0),

corelib/include/rtabmap/core/Memory.h

@@ -82,6 +82,7 @@ public:
 	void joinTrashThread();
 	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
 	void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
+	void removeAllVirtualLinks();
 	std::map<int, int> getNeighborsId(int signatureId,
 			int margin,
 			int maxCheckedInDatabase = -1,

corelib/include/rtabmap/core/Parameters.h

@@ -175,7 +175,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
 	RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,   	 bool, true, "Add column header description to log files.");
-	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.")
+	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
@@ -286,6 +286,8 @@ class RTABMAP_EXP Parameters
 	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).");
+	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 1.0, "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, MaxAnticipatedNodes,  unsigned int, 10, "Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by \"Rtabmap/MaxRetrieved\").");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -71,10 +71,11 @@ public:
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
-	int getLoopClosureId() const;
-	int getRetrievedId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
 	int getLastLocationId() const;
-	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM() const; // working memory
 	std::set<int> getSTM() const; // short-term memory
 	int getWMSize() const; // working memory size
@@ -90,6 +91,7 @@ public:
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -115,8 +117,13 @@ public:
 			std::map<int, int> & mapIds,
 			bool optimized,
 			bool global);
+	void clearPath();
+	std::list<std::pair<int, Transform> > computePath(int targetNode);
+	const std::vector<int> & getPath() const {return _path;}
+	int getPathGoalId() const;
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
+	std::map<int, Transform> getWMPosesInRadius(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;
@@ -126,6 +133,7 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	void updateGoalIndex();
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -166,10 +174,11 @@ private:
 	int _reextractFeatureType;
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
+	float _goalReachedRadius; // meters
+	unsigned int _maxAnticipatedNodes;
 
-	int _lcHypothesisId;
-	float _lcHypothesisValue;
-	int _retrievedId;
+	std::pair<int, float> _loopClosureHypothesis;
+	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
 
 	// Abstract classes containing all loop closure
@@ -193,6 +202,12 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+
+	// Planning stuff
+	std::vector<int> _path;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/Signature.h

@@ -89,6 +89,7 @@ public:
 
 	void removeLinks();
 	void removeLink(int idTo);
+	void removeVirtualLinks();
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}