Updated how locations are retrieved on a planned path or near based on space if no path is activated.

Added mirroring option to OpenNI2 camera
Removed "RGBD/MaxAnticipatedNodes" and "RGBD/GoalMaxDistance" parameters
Renamed "RGBD/LocalLoopDetectionRadius" to "RGBD/LocalRadius"
Added "RGBD/MaxLocalRetrieved" parameter
GUI: fixed OpenNI 2 selection from the MainWindow, added OpenNI2 under Kinect menu

corelib/include/rtabmap/core/CameraRGBD.h

@@ -199,7 +199,8 @@ public:
 	static bool exposureGainAvailable();
 
 public:
-	CameraOpenNI2(float imageRate = 0,
+	CameraOpenNI2(const std::string & deviceId = "",
+					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity(),
 					float fx = 0.0f,
 					float fy = 0.0f,
@@ -213,6 +214,7 @@ public:
 	bool setAutoExposure(bool enabled);
 	bool setExposure(int value);
 	bool setGain(int value);
+	bool setMirroring(bool enabled);
 
 protected:
 	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -223,6 +225,7 @@ private:
 	openni::VideoStream * _depth;
 	float _depthFx;
 	float _depthFy;
+	std::string _deviceId;
 };
 
 

corelib/include/rtabmap/core/Graph.h

@@ -77,6 +77,14 @@ bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
 		std::map<int, Transform> & poses,
 		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
 
+/**
+ * Get only the the most recent or older poses in the defined radius.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
+ * @return A map containing only most recent or older poses in the the defined radius
+ */
 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,
@@ -87,7 +95,7 @@ std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
  * Get all neighbor nodes in a fixed radius around each pose.
  * @param poses The poses
  * @param radius Radius (m) of the search for near neighbors
- * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
  * @return A map between each pose id and its neighbors found in the radius
  */
 std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
@@ -115,12 +123,26 @@ int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);
 
+/**
+ * Get nodes near the query
+ * @param nodeId the query id
+ * @param nodes the nodes to search for
+ * @param maxNearestNeighbors Maximum nearest neighbor to get. 0 means all.
+ * @param radius radius to search for (m)
+ * @return the nodes with squared distance to query node.
+ */
 std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		int maxNearestNeighbors,
 		float radius);
 
+float RTABMAP_EXP computePathLength(
+		const std::vector<std::pair<int, Transform> > & path,
+		unsigned int fromIndex = 0,
+		unsigned int toIndex = 0);
+
+
 } /* namespace graph */
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -290,14 +290,13 @@ class RTABMAP_EXP Parameters
 	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, 0.5, "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\").");
 	RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked,  bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
-	RTABMAP_PARAM(RGBD, GoalMaxDistance,  float, 0, "Maximum distance (m) of the target goal from the graph (0 means infinity). If the goal is too far from the graph, the plan is aborted. Also when set, the next goal in the graph can't be farther than this distance from the current position.");
+	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
+	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
 
 	// 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, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
 	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.")
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -93,7 +93,7 @@ public:
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
 	float getGoalReachedRadius() const {return _goalReachedRadius;}
-	float getGoalMaxDistance() const {return _goalMaxDistance;}
+	float getLocalRadius() const {return _localRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
@@ -128,7 +128,6 @@ public:
 	int getPathCurrentGoalId() const;
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
-	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,
@@ -156,6 +155,7 @@ private:
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
+	unsigned int _maxLocalRetrieved;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
 	bool _statisticLoggedHeaders;
@@ -168,7 +168,7 @@ private:
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
-	float _localDetectRadius;
+	float _localRadius;
 	float _localDetectMaxNeighbors;
 	int _localDetectMaxDiffID;
 	int _toroIterations;
@@ -182,9 +182,7 @@ private:
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
-	unsigned int _maxAnticipatedNodes;
 	bool _planWithNearNodesLinked;
-	float _goalMaxDistance;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;