Increased database version to 0.8.11 (new Depth.data2d_max_pts column). Updated how local loop closure detection in space is done. Update GraphViewer with local radius ellipse and current goal node color. MainWindow saving/loading figures automatically accordingly to the previous session saved.

corelib/include/rtabmap/core/DBReader.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 
 #include <set>
+#include <list>
 
 namespace rtabmap {
 
@@ -50,6 +51,10 @@ public:
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false);
+	DBReader(const std::list<std::string> & databasePaths,
+			 float frameRate = 0.0f,
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -61,7 +66,7 @@ protected:
 	virtual void mainLoop();
 
 private:
-	std::string _path;
+	std::list<std::string> _paths;
 	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;

corelib/include/rtabmap/core/Graph.h

@@ -207,7 +207,10 @@ int RTABMAP_EXP findNearestNode(
 std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
-		int maxNearestNeighbors,
+		float radius);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
 		float radius);
 
 float RTABMAP_EXP computePathLength(

corelib/include/rtabmap/core/Memory.h

@@ -84,12 +84,18 @@ public:
 	bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
 	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
 	void removeAllVirtualLinks();
-	std::map<int, int> getNeighborsId(int signatureId,
-			int margin,
+	std::map<int, int> getNeighborsId(
+			int signatureId,
+			int maxGraphDepth,
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
+	std::map<int, float> getNeighborsIdRadius(
+			int signatureId,
+			float radius,
+			const std::map<int, Transform> & optimizedPoses,
+			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
 	void removeLink(int idA, int idB);
 
@@ -224,6 +230,7 @@ private:
 	bool _localSpaceLinksKeptInWM;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
+	bool _rehearsalWeightIgnoredWhileMoving;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Parameters.h

@@ -191,6 +191,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
 	RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false,  "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
+	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, true, "When the robot is moving, weights are not updated on rehearsal.");
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
@@ -297,8 +298,8 @@ class RTABMAP_EXP Parameters
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 50,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.25, "Path filtering radius.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 20,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
 
 	// Graph optimization

corelib/include/rtabmap/core/Rtabmap.h

@@ -184,7 +184,7 @@ private:
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localRadius;
-	int _localDetectMaxDiffID;
+	int _localDetectMaxGraphDepth;
 	float _localPathFilteringRadius;
 	bool _localPathOdomPosesUsed;
 	std::string _databasePath;

corelib/include/rtabmap/core/Signature.h

@@ -129,7 +129,7 @@ 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 setLaserScanCompressed(const cv::Mat & bytes, int maxPts = 0) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
+	void setLaserScanCompressed(const cv::Mat & bytes, int maxPts) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
 	void setLocalTransform(const Transform & t) {_localTransform = t;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
@@ -147,7 +147,7 @@ public:
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
 	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts = 0) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
+	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
 	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
 	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 

corelib/include/rtabmap/core/Statistics.h

@@ -66,7 +66,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(LocalLoop, Time_closures,);
 	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
 	RTABMAP_STATS(LocalLoop, Space_paths,);
-	RTABMAP_STATS(LocalLoop, Space_closures_added,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_visually,);
 	RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
 
 	RTABMAP_STATS(OdomCorrection, Accepted,);
@@ -81,6 +81,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Local_graph_size,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);
@@ -147,6 +148,7 @@ public:
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
 	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
+	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -169,6 +171,7 @@ public:
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
 	const std::vector<int> & localPath() const {return _localPath;}
+	int currentGoalId() const {return _currentGoalId;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -199,6 +202,7 @@ private:
 	std::map<int, float> _rawLikelihood;
 
 	std::vector<int> _localPath;
+	int _currentGoalId;
 
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}