Increased version to 0.10.10. Database: added user_data field for links. DatabaseViewer: showing all scans of a local loop closure. Added parameters RGBD/PlanLinearVelocity and RGBD/PlanAngularVelocity. Updated how variance is set on links. MainWindow: added Send Waypoints action and goal can be either an ID or a label.

corelib/include/rtabmap/core/DBReader.h

@@ -50,11 +50,13 @@ public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 bool ignoreGoalDelay = false);
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 bool ignoreGoalDelay = false);
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -70,6 +72,7 @@ private:
 	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
+	bool _goalsIgnored;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Graph.h

@@ -307,7 +307,9 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		int toId,
 		const Memory * memory,
 		bool lookInDatabase = true,
-		bool updateNewCosts = false);
+		bool updateNewCosts = false,
+		float linearVelocity = 0.0f,   // m/sec
+		float angularVelocity = 0.0f); // rad/sec
 
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
@@ -335,6 +337,10 @@ float RTABMAP_EXP computePathLength(
 		unsigned int fromIndex = 0,
 		unsigned int toIndex = 0);
 
+std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
+		std::map<int, Transform> poses,
+		const std::multimap<int, Link> & links);
+
 
 } /* namespace graph */
 

corelib/include/rtabmap/core/Link.h

@@ -29,8 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LINK_H_
 
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UMath.h>
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap {
@@ -39,38 +37,20 @@ class Link
 {
 public:
 	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
-	Link() :
-		from_(0),
-		to_(0),
-		type_(kUndef),
-		infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
-	{
-	}
+	Link();
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
-		from_(from),
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setInfMatrix(infMatrix);
-	}
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
+			const cv::Mat & userData = cv::Mat());
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
 			double rotVariance,
-			double transVariance) :
-		from_(from),
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setVariance(rotVariance, transVariance);
-	}
+			double transVariance,
+			const cv::Mat & userData = cv::Mat());
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
 
@@ -79,69 +59,25 @@ public:
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
 	const cv::Mat & infMatrix() const {return infMatrix_;}
-	double rotVariance() const
-	{
-		double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
-		UASSERT(min > 0.0);
-		return 1.0/min;
-	}
-	double transVariance() const
-	{
-		double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
-		UASSERT(min > 0.0);
-		return 1.0/min;
-	}
+	double rotVariance() const;
+	double transVariance() const;
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setInfMatrix(const cv::Mat & infMatrix) {
-		UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
-		infMatrix_ = infMatrix;
-	}
-	void setVariance(double rotVariance, double transVariance) {
-		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
-		UASSERT(uIsFinite(transVariance) && transVariance>0);
-		infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
-		infMatrix_.at<double>(0,0) = 1.0/transVariance;
-		infMatrix_.at<double>(1,1) = 1.0/transVariance;
-		infMatrix_.at<double>(2,2) = 1.0/transVariance;
-		infMatrix_.at<double>(3,3) = 1.0/rotVariance;
-		infMatrix_.at<double>(4,4) = 1.0/rotVariance;
-		infMatrix_.at<double>(5,5) = 1.0/rotVariance;
-	}
+	void setInfMatrix(const cv::Mat & infMatrix);
+	void setVariance(double rotVariance, double transVariance);
 
-	Link merge(const Link & link, Type outputType) const
-	{
-		UASSERT(to_ == link.from());
-		UASSERT(outputType != Link::kUndef);
-		UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
-		UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
-		UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
-		return Link(
-				from_,
-				link.to(),
-				outputType,
-				transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
-				transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
-	}
+	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
+	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
+	void uncompressUserData();
+	cv::Mat uncompressUserDataConst() const;
 
-	Link inverse() const
-	{
-		return Link(
-				to_,
-				from_,
-				type_,
-				transform_.isNull()?Transform():transform_.inverse(),
-				transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
-	}
+	Link merge(const Link & link, Type outputType) const;
+	Link inverse() const;
 
 private:
 	int from_;
@@ -149,6 +85,10 @@ private:
 	Transform transform_;
 	Type type_;
 	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -293,8 +293,9 @@ class RTABMAP_EXP Parameters
 	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, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
-	RTABMAP_PARAM(RGBD, PlanVirtualLinks,         bool, true,  "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
 	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	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.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -204,9 +204,10 @@ private:
 	float _reextractMaxDepth;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
-	bool _planVirtualLinks;
 	bool _goalsSavedInUserData;
 	int _pathStuckIterations;
+	float _pathLinearVelocity;
+	float _pathAngularVelocity;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -234,6 +234,16 @@ private:
 	std::string _label;
 };
 
+class RtabmapGoalStatusEvent : public UEvent
+{
+public:
+	RtabmapGoalStatusEvent(int status):
+		UEvent(status){}
+
+	virtual ~RtabmapGoalStatusEvent() {}
+	virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");}
+};
+
 } // namespace rtabmap
 
 #endif /* RTABMAPEVENT_H_ */