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.

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 10)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

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() :
+	Link();
-		from_(0),
-		to_(0),
-		type_(kUndef),
-		infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
-	{
-	}
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
-			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
-		from_(from),
+			const cv::Mat & userData = cv::Mat());
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setInfMatrix(infMatrix);
-	}
 	Link(int from,
 			int to,
 			Type type,
 			const Transform & transform,
 			double rotVariance,
-			double transVariance) :
+			double transVariance,
-		from_(from),
+			const cv::Mat & userData = cv::Mat());
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-		setVariance(rotVariance, transVariance);
-	}
 
 	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 rotVariance() const;
-	{
+	double transVariance() 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;
-	}
 
 	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) {
+	void setInfMatrix(const cv::Mat & infMatrix);
-		UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
+	void setVariance(double rotVariance, double transVariance);
-		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;
-	}
 
-	Link merge(const Link & link, Type outputType) const
+	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.
-		UASSERT(to_ == link.from());
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
-		UASSERT(outputType != Link::kUndef);
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
-		UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
+	void uncompressUserData();
-		UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
+	cv::Mat uncompressUserDataConst() const;
-		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());
-	}
 
-	Link inverse() const
+	Link merge(const Link & link, Type outputType) const;
-	{
+	Link inverse() const;
-		return Link(
-				to_,
-				from_,
-				type_,
-				transform_.isNull()?Transform():transform_.inverse(),
-				transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
-	}
 
 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_ */

corelib/src/CMakeLists.txt

@@ -42,6 +42,7 @@ SET(SRC_FILES
 	SensorData.cpp
 	Graph.cpp
 	Compression.cpp
+	Link.cpp
 	
 	Odometry.cpp
 	OdometryThread.cpp

corelib/src/DBDriverSqlite3.cpp

@@ -1552,7 +1552,11 @@ void DBDriverSqlite3::loadLinksQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		{
+			query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link ";
+		}
+		else if(uStrNumCmp(_version, "0.8.4") >= 0)
 		{
 			query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
 		}
@@ -1618,7 +1622,20 @@ void DBDriverSqlite3::loadLinksQuery(
 			{
 				rotVariance = sqlite3_column_double(ppStmt, index++);
 				transVariance = sqlite3_column_double(ppStmt, index++);
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+
+				cv::Mat userDataCompressed;
+				if(uStrNumCmp(_version, "0.10.10") >= 0)
+				{
+					const void * data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					//Create the userData
+					if(dataSize>4 && data)
+					{
+						userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+					}
+				}
+
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed)));
 			}
 			else if(uStrNumCmp(_version, "0.7.4") >= 0)
 			{
@@ -1658,7 +1675,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;
 
-		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		if(uStrNumCmp(_version, "0.10.10") >= 0)
+		{
+			query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else if(uStrNumCmp(_version, "0.8.4") >= 0)
 		{
 			query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
 				  << "WHERE from_id = ? "
@@ -1702,10 +1725,22 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
+				cv::Mat userDataCompressed;
 				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
 					rotVariance = sqlite3_column_double(ppStmt, index++);
 					transVariance = sqlite3_column_double(ppStmt, index++);
+
+					if(uStrNumCmp(_version, "0.10.10") >= 0)
+					{
+						const void * data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						//Create the userData
+						if(dataSize>4 && data)
+						{
+							userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+						}
+					}
 				}
 				else if(uStrNumCmp(_version, "0.7.4") >= 0)
 				{
@@ -1729,11 +1764,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				{
 					if(uStrNumCmp(_version, "0.7.4") >= 0)
 					{
-						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed));
 					}
 					else // neighbor is 0, loop closures are 1 and 2 (child)
 					{
-						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed));
 					}
 				}
 				else
@@ -2516,7 +2551,11 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepLink() const
 {
-	if(uStrNumCmp(_version, "0.8.4") >= 0)
+	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	{
+		return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform, user_data) VALUES(?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
 		return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
 	}
@@ -2571,6 +2610,20 @@ void DBDriverSqlite3::stepLink(
 	rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.10.10") >= 0)
+	{
+		// user_data
+		if(!link.userDataCompressed().empty())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 

corelib/src/DBReader.cpp

@@ -45,11 +45,13 @@ namespace rtabmap {
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
-				   bool ignoreGoalDelay) :
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
 	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0)
@@ -59,11 +61,13 @@ DBReader::DBReader(const std::string & databasePath,
 DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   float frameRate,
 				   bool odometryIgnored,
-				   bool ignoreGoalDelay) :
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
    _paths(databasePaths),
    _frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0)
@@ -159,7 +163,8 @@ void DBReader::mainLoop()
 		{
 			odom.data().setStamp(UTimer::now());
 		}
-		if(odom.data().userDataRaw().type() == CV_8SC1 &&
+		if(!_goalsIgnored &&
+		   odom.data().userDataRaw().type() == CV_8SC1 &&
 		   odom.data().userDataRaw().cols >= 7 && // including null str ending
 		   odom.data().userDataRaw().rows == 1 &&
 		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)

corelib/src/Graph.cpp

@@ -1890,6 +1890,7 @@ public:
 	void setFromId(int fromId) {fromId_ = fromId;}
 	void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
 	void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
+	void setPose(const Transform & pose) {pose_ = pose;}
 
 private:
 	int id_;
@@ -2021,12 +2022,21 @@ std::list<std::pair<int, Transform> > computePath(
 		int toId,
 		const Memory * memory,
 		bool lookInDatabase,
-		bool updateNewCosts)
+		bool updateNewCosts,
+		float linearVelocity,  // m/sec
+		float angularVelocity) // rad/sec
 {
 	UASSERT(memory!=0);
 	UASSERT(fromId>=0);
 	UASSERT(toId>=0);
 	std::list<std::pair<int, Transform> > path;
+	UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
+			fromId,
+			toId,
+			lookInDatabase?1:0,
+			updateNewCosts?1:0,
+			linearVelocity,
+			angularVelocity);
 
 	std::multimap<int, Link> allLinks;
 	if(lookInDatabase)
@@ -2097,11 +2107,34 @@ std::list<std::pair<int, Transform> > computePath(
 		}
 		for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
+			Transform nextPose = currentNode->pose()*iter->second.transform();
+			float cost = 0.0f;
+			if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
+			{
+				// use distance only
+				cost = iter->second.transform().getNorm();
+			}
+			else // use time
+			{
+				if(linearVelocity > 0.0f)
+				{
+					cost += iter->second.transform().getNorm()/linearVelocity;
+				}
+				if(angularVelocity > 0.0f)
+				{
+					Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
+					Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
+					float angle = pcl::getAngle3D(v1, v2);
+					cost += angle / angularVelocity;
+				}
+			}
+
 			std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
 			if(nodeIter == nodes.end())
 			{
-				Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform());
+				Node n(iter->second.to(), currentNode->id(), nextPose);
-				n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm());
+
+				n.setCostSoFar(currentNode->costSoFar() + cost);
 				nodes.insert(std::make_pair(iter->second.to(), n));
 				if(updateNewCosts)
 				{
@@ -2114,9 +2147,12 @@ std::list<std::pair<int, Transform> > computePath(
 			}
 			else if(updateNewCosts && nodeIter->second.isOpened())
 			{
-				float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
+				float newCostSoFar = currentNode->costSoFar() + cost;
 				if(nodeIter->second.costSoFar() > newCostSoFar)
 				{
+					// update pose with new link
+					nodeIter->second.setPose(nextPose);
+
 					// update the cost in the priority queue
 					for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
 					{
@@ -2132,6 +2168,61 @@ std::list<std::pair<int, Transform> > computePath(
 			}
 		}
 	}
+
+	// Debugging stuff
+	if(ULogger::level() == ULogger::kDebug)
+	{
+		std::stringstream stream;
+		std::vector<int> linkTypes(Link::kUndef, 0);
+		std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
+		float length = 0.0f;
+		for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
+		{
+			if(iter!=path.begin())
+			{
+				stream << ",";
+			}
+
+			if(previousIter!=path.end())
+			{
+				//UDEBUG("current  %d = %s", iter->first, iter->second.prettyPrint().c_str());
+				if(allLinks.size())
+				{
+					std::multimap<int, Link>::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first);
+					if(jter != allLinks.end())
+					{
+						//Transform nextPose = iter->second;
+						//Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
+						//Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
+						//float angle = pcl::getAngle3D(v1, v2);
+						//float cost = angle ;
+						//UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
+
+						UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kUndef);
+						++linkTypes[jter->second.type()];
+						stream << "[" << jter->second.type() << "]";
+						length += jter->second.transform().getNorm();
+					}
+				}
+			}
+
+			stream << iter->first;
+
+			previousIter=iter;
+		}
+		UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str());
+		std::stringstream streamB;
+		for(unsigned int i=0; i<linkTypes.size(); ++i)
+		{
+			if(i > 0)
+			{
+				streamB << " ";
+			}
+			streamB << i << "=" << linkTypes[i];
+		}
+		UDEBUG("Link types = %s", streamB.str().c_str());
+	}
+
 	return path;
 }
 
@@ -2302,6 +2393,39 @@ float computePathLength(
 	return length;
 }
 
+// return all paths linked only by neighbor links
+std::list<std::map<int, Transform> > getPaths(
+		std::map<int, Transform> poses,
+		const std::multimap<int, Link> & links)
+{
+	std::list<std::map<int, Transform> > paths;
+	if(poses.size() && links.size())
+	{
+		// Segment poses connected only by neighbor links
+		while(poses.size())
+		{
+			std::map<int, Transform> path;
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
+			{
+				std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
+				if(path.size() == 0 || (jter != links.end() && jter->second.type() == Link::kNeighbor))
+				{
+					path.insert(*iter);
+					poses.erase(iter++);
+				}
+				else
+				{
+					break;
+				}
+			}
+			UASSERT(path.size());
+			paths.push_back(path);
+		}
+
+	}
+	return paths;
+}
+
 } /* namespace graph */
 
 } /* namespace rtabmap */

corelib/src/Link.cpp

@@ -0,0 +1,198 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Link.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/core/Compression.h>
+
+namespace rtabmap {
+
+Link::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,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setInfMatrix(infMatrix);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+Link::Link(int from,
+		int to,
+		Type type,
+		const Transform & transform,
+		double rotVariance,
+		double transVariance,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setVariance(rotVariance, transVariance);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+double Link::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 Link::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;
+}
+
+void Link::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 Link::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 Link::setUserDataRaw(const cv::Mat & userDataRaw)
+{
+	if(!_userDataRaw.empty())
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = userDataRaw;
+}
+
+void Link::setUserData(const cv::Mat & userData)
+{
+	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = cv::Mat();
+	_userDataCompressed = cv::Mat();
+
+	if(!userData.empty())
+	{
+		if(userData.type() == CV_8UC1) // Bytes
+		{
+			_userDataCompressed = userData; // assume compressed
+		}
+		else
+		{
+			_userDataRaw = userData;
+			_userDataCompressed = compressData2(userData);
+		}
+	}
+}
+
+void Link::uncompressUserData()
+{
+	cv::Mat dataRaw = uncompressUserDataConst();
+	if(!dataRaw.empty() && _userDataRaw.empty())
+	{
+		_userDataRaw = dataRaw;
+	}
+}
+
+cv::Mat Link::uncompressUserDataConst() const
+{
+	if(!_userDataRaw.empty())
+	{
+		return _userDataRaw;
+	}
+	return uncompressData(_userDataCompressed);
+}
+
+Link 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());
+}
+
+Link Link::inverse() const
+{
+	return Link(
+			to_,
+			from_,
+			type_,
+			transform_.isNull()?Transform():transform_.inverse(),
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
+}
+
+}

corelib/src/Memory.cpp

@@ -1090,7 +1090,8 @@ std::map<int, float> Memory::getNeighborsIdRadius(
 				for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
 				{
 					if(!uContains(ids, iter->first) &&
-						uContains(optimizedPoses, iter->first))
+						uContains(optimizedPoses, iter->first) &&
+						iter->second.type()!=Link::kVirtualClosure)
 					{
 						const Transform & t = optimizedPoses.at(iter->first);
 						UASSERT(!t.isNull());
@@ -2224,7 +2225,7 @@ Transform Memory::computeVisualTransform(
 	}
 	if(varianceOut)
 	{
-		*varianceOut = variance;
+		*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
 	}
 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
@@ -2428,7 +2429,7 @@ Transform Memory::computeIcpTransform(
 
 				if(varianceOut)
 				{
-					*varianceOut = variance;
+					*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
 				}
 				if(correspondencesOut)
 				{
@@ -2518,7 +2519,7 @@ Transform Memory::computeIcpTransform(
 				bool hasConverged = false;
 				float correspondencesRatio = 0.0f;
 				int correspondences = 0;
-				double variance = 1;
+				double variance = 1.0;
 				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
 				icpT = util3d::icp2D(
 						newCloudVoxelized,
@@ -2567,7 +2568,6 @@ Transform Memory::computeIcpTransform(
 							newCloud = newCloudRegistered;
 						}
 
-						pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
 						util3d::computeVarianceAndCorrespondences(
 								newCloud,
 								oldCloud,
@@ -2592,12 +2592,12 @@ Transform Memory::computeIcpTransform(
 								hasConverged?"true":"false",
 								variance,
 								correspondences,
-								(int)(newS.sensorData().laserScanMaxPts()),
+								newS.sensorData().laserScanMaxPts()?newS.sensorData().laserScanMaxPts():(int)(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()),
 								correspondencesRatio*100.0f);
 
 						if(varianceOut)
 						{
-							*varianceOut = variance;
+							*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
 						}
 						if(correspondencesOut)
 						{
@@ -2627,6 +2627,21 @@ Transform Memory::computeIcpTransform(
 							hasConverged?"true":"false", variance);
 					UINFO(msg.c_str());
 				}
+
+				// still compute the variance for information
+				if(variance == 1 && varianceOut)
+				{
+					util3d::computeVarianceAndCorrespondences(
+						newCloudVoxelized,
+						oldCloudVoxelized,
+						_icpMaxCorrespondenceDistance,
+						variance,
+						correspondences);
+					if(variance > 0)
+					{
+						*varianceOut = variance;
+					}
+				}
 			}
 			else
 			{
@@ -2747,6 +2762,23 @@ Transform Memory::computeScanMatchingTransform(
 		//}
 
 		if(!icpT.isNull() && hasConverged)
+		{
+			float ix,iy,iz, iroll,ipitch,iyaw;
+			icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
+			if((_icpMaxTranslation>0.0f &&
+				(fabs(ix) > _icpMaxTranslation ||
+				 fabs(iy) > _icpMaxTranslation ||
+				 fabs(iz) > _icpMaxTranslation))
+				||
+				(_icpMaxRotation>0.0f &&
+				 (fabs(iroll) > _icpMaxRotation ||
+				  fabs(ipitch) > _icpMaxRotation ||
+				  fabs(iyaw) > _icpMaxRotation)))
+			{
+				msg = uFormat("Cannot compute transform (ICP correction too large)");
+				UINFO(msg.c_str());
+			}
+			else
 			{
 				if(_icp2VoxelSize <= _laserScanVoxelSize)
 				{
@@ -2767,7 +2799,7 @@ Transform Memory::computeScanMatchingTransform(
 						correspondences);
 				if(variance)
 				{
-				*variance = v;
+					*variance = v>0.0f?v:0.0001; // epsilon if exact transform
 				}
 
 				// verify if there enough correspondences
@@ -2784,9 +2816,9 @@ Transform Memory::computeScanMatchingTransform(
 				}
 
 				UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
-					variance?*variance:-1,
+						v,
 						correspondences,
-					(int)newCloud->size(),
+						newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(),
 						correspondencesRatio*100.0f);
 
 				if(inliers)
@@ -2803,11 +2835,12 @@ Transform Memory::computeScanMatchingTransform(
 					msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)",
 						variance?*variance:-1,
 						correspondences,
-					(int)newCloud->size(),
+						newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(),
 						correspondencesRatio);
 					UINFO(msg.c_str());
 				}
 			}
+		}
 		else
 		{
 			msg = uFormat("Constraints failed... hasConverged=%s",
@@ -4292,7 +4325,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 		if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
-			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion.");
+			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
 			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 		}
 

corelib/src/OdometryMono.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UMath.h"
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/video/tracking.hpp>

corelib/src/Rtabmap.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/BayesFilter.h"
+#include "rtabmap/core/Compression.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -109,9 +110,10 @@ Rtabmap::Rtabmap() :
 	_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
-	_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
 	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
 	_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
+	_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
+	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -415,9 +417,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
-	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
+	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
+	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1081,12 +1084,14 @@ bool Rtabmap::process(
 				Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
 				if(!t.isNull())
 				{
-					UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
+					UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
 							signature->id(),
 							oldId,
+							variance,
 							signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 							t.prettyPrint().c_str());
-					_memory->updateLink(signature->id(), oldId, t, variance>0?variance:0.0001, variance>0?variance:0.0001);
+					UASSERT(variance > 0.0);
+					_memory->updateLink(signature->id(), oldId, t, variance, variance);
 
 					if(_optimizeFromGraphEnd)
 					{
@@ -1107,6 +1112,11 @@ bool Rtabmap::process(
 				else
 				{
 					UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
+					if(variance > 0)
+					{
+						double sqrtVar = sqrt(variance);
+						_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
+					}
 				}
 				statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0);
 				statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
@@ -1191,7 +1201,8 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001)))
+						UASSERT(variance > 0.0);
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1807,7 +1818,8 @@ bool Rtabmap::process(
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001));
+			UASSERT(variance > 0.0);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
 			if(!rejectedHypothesis)
 			{
 				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1850,41 +1862,45 @@ bool Rtabmap::process(
 				//
 				// 1) compare visually with nearest locations
 				//
-				float r = _localRadius;
+				UDEBUG("Proximity detection (local loop closure in SPACE using matching images)");
-				if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius)
-				{
-					r = _localPathFilteringRadius;
-				}
-
 				std::map<int, float> nearestIds;
 				if(_memory->isIncremental())
 				{
-					nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
+					nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _localDetectMaxGraphDepth);
 				}
 				else
 				{
-					nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, r);
+					nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
 				}
+				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
 				for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
+				{
+					if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
 					{
 						nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
 					}
+				}
+				UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
+
 				// segment poses by paths, only one detection per path
 				std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
-				for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
+				UDEBUG("nearestPaths=%d", (int)nearestPaths.size());
+
+				for(std::list<std::map<int, Transform> >::const_iterator iter=nearestPaths.begin();
 					iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
 					++iter)
 				{
-					std::map<int, Transform> & path = *iter;
+					const std::map<int, Transform> & path = *iter;
 					UASSERT(path.size());
 					//find the nearest pose on the path
 					int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 					UASSERT(nearestId > 0);
 
-					// nearest pose must not be linked to current location, and not in STM
+					// nearest pose must not be linked to current location and enough
 					if(!signature->hasLink(nearestId) &&
-					   _memory->getStMem().find(nearestId) == _memory->getStMem().end())
+						(_localPathFilteringRadius <= 0.0f ||
+						 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
 					{
 						double variance = 1.0;
 						Transform transform;
@@ -1952,12 +1968,15 @@ bool Rtabmap::process(
 							transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
 						}
 						if(!transform.isNull())
+						{
+							if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius)
 							{
 								UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
 										signature->id(),
 										nearestId,
 										transform.prettyPrint().c_str());
-							_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001));
+								UASSERT(variance > 0.0);
+								_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
 								loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 								if(_loopClosureHypothesis.first == 0)
@@ -1966,12 +1985,20 @@ bool Rtabmap::process(
 									lastLocalSpaceClosureId = nearestId;
 								}
 							}
+							else
+							{
+								UWARN("Ignoring local loop closure with %d because resulting "
+									  "transform is to large!? (%fm > %fm)",
+										nearestId, transform.getNorm(), _localPathFilteringRadius);
+							}
+						}
 					}
 				}
 
 				//
 				// 2) compare locally with nearest locations by scan matching
 				//
+				UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)");
 				if( !signature->sensorData().laserScanCompressed().empty() &&
 					(_memory->isIncremental() || lastLocalSpaceClosureId == 0))
 				{
@@ -1979,18 +2006,10 @@ bool Rtabmap::process(
 					// closures if we are already localized by at least one
 					// local visual closure above.
 
-					std::map<int, Transform> forwardPoses;
+					localSpacePaths = (int)nearestPaths.size();
-					forwardPoses = this->getForwardWMPoses(
-							signature->id(),
-							0,
-							_localRadius,
-							_localDetectMaxGraphDepth);
 
-					std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
+					for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin();
-					localSpacePaths = (int)forwardPaths.size();
+							iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
-
-					for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin();
-							iter!=forwardPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
 							++iter)
 					{
 						std::map<int, Transform> & path = *iter;
@@ -1999,6 +2018,7 @@ bool Rtabmap::process(
 						//find the nearest pose on the path
 						int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 						UASSERT(nearestId > 0);
+						UDEBUG("Path %d distance=%fm", nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
 
 						// nearest pose must be close and not linked to current location
 						if(!signature->hasLink(nearestId) &&
@@ -2021,7 +2041,7 @@ bool Rtabmap::process(
 							if(_localPathFilteringRadius > 0.0f)
 							{
 								// path filtering
-								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
+								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
 								// make sure the nearest and farthest poses are still here
 								filteredPath.insert(*path.find(nearestId));
 								filteredPath.insert(*path.begin());
@@ -2036,15 +2056,43 @@ bool Rtabmap::process(
 								//The nearest will be the reference for a loop closure transform
 								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
 								{
-									Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0);
+									double variance = 1.0;
+									Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
 									if(!transform.isNull())
+									{
+										if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius)
 										{
 											UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
 													signature->id(),
 													nearestId,
 													transform.prettyPrint().c_str());
+
+											cv::Mat scanMatchingIds;
+											bool _scanMatchingIdsSavedInUserData = true;
+											if(_scanMatchingIdsSavedInUserData)
+											{
+												std::stringstream stream;
+												stream << "SCANS:";
+												for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
+												{
+													if(iter->first!=signature->id())
+													{
+														if(iter != path.begin())
+														{
+															stream << ";";
+														}
+														stream << uNumber2Str(iter->first);
+													}
+												}
+												std::string scansStr = stream.str();
+												scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
+												scanMatchingIds = compressData2(scanMatchingIds); // compressed
+											}
+
 											// set Identify covariance for laser scan matching only
-										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1));
+											UASSERT(variance>0.0);
+											double sqrtVar = sqrt(variance);
+											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
 											loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 											++localSpaceClosuresAddedByICPOnly;
@@ -2055,11 +2103,22 @@ bool Rtabmap::process(
 												lastLocalSpaceClosureId = nearestId;
 											}
 										}
+										else
+										{
+											UWARN("Ignoring local loop closure with %d because resulting "
+												  "transform is to large!? (%fm > %fm)",
+													nearestId, transform.getNorm(), _localPathFilteringRadius);
 										}
 									}
 								}
 							}
 						}
+						else
+						{
+							UDEBUG("Path %d ignored", nearestId);
+						}
+					}
+				}
 			}
 		}
 	}
@@ -2157,6 +2216,9 @@ bool Rtabmap::process(
 			{
 				const Link * maxLinearLink = 0;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
+				{
+					// ignore links with high variance
+					if(iter->second.transVariance() < 1.0)
 					{
 						Transform t1 = uValue(poses, iter->second.from(), Transform());
 						Transform t2 = uValue(poses, iter->second.to(), Transform());
@@ -2171,18 +2233,20 @@ bool Rtabmap::process(
 							maxLinearLink = &iter->second;
 						}
 					}
+				}
 
 				if(maxLinearError > _optimizationMaxLinearError)
 				{
 					UWARN("Rejecting all added loop closures (%d) in this "
 						  "iteration because a wrong loop closure has been "
 						  "detected after graph optimization, resulting in "
-						  "a maximum graph error of %f m (edge %d->%d). The "
+						  "a maximum graph error of %f m (edge %d->%d, type=%d). The "
 						  "maximum error parameter is %f m.",
 						  (int)loopClosureLinksAdded.size(),
 						  maxLinearError,
 						  maxLinearLink->from(),
 						  maxLinearLink->to(),
+						  maxLinearLink->type(),
 						  _optimizationMaxLinearError);
 					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
 					{
@@ -2819,7 +2883,6 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 	return poses;
 }
 
-// Get paths in front of the robot, returned optimized poses
 std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
 {
 	std::list<std::map<int, Transform> > paths;
@@ -3234,12 +3297,14 @@ bool Rtabmap::computePath(int targetNode, bool global)
 		}
 		if(currentNode && targetNode)
 		{
-
 			std::list<std::pair<int, Transform> > path = graph::computePath(
 					currentNode,
 					targetNode,
 					_memory,
-					global);
+					global,
+					false,
+					_pathLinearVelocity,
+					_pathAngularVelocity);
 
 			//transform in current referential
 			Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity());
@@ -3353,20 +3418,6 @@ bool Rtabmap::computePath(const Transform & targetPose)
 				currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
 			}
 
-			// Add links between neighbor nodes in the goal radius.
-			if(_planVirtualLinks)
-			{
-				std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
-				for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
-				{
-					if(graph::findLink(links, iter->first, iter->second) == links.end())
-					{
-						links.insert(*iter);
-						links.insert(std::make_pair(iter->second, iter->first)); // <->
-					}
-				}
-			}
-
 			UINFO("Computing path from location %d to %d", currentNode, nearestId);
 			UTimer timer;
 			_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));
@@ -3531,7 +3582,7 @@ void Rtabmap::updateGoalIndex()
 						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
 							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance
+							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
 							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}

corelib/src/RtabmapThread.cpp

@@ -463,12 +463,19 @@ void RtabmapThread::process()
 	{
 		if(_rtabmap->getMemory())
 		{
+			bool wasPlanning = _rtabmap->getPath().size()>0;
 			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
 				ULOGGER_DEBUG("posting statistics_ event...");
 				this->post(new RtabmapEvent(stats));
+
+				if(wasPlanning && _rtabmap->getPath().size() == 0)
+				{
+					// Goal reached or failed
+					this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus()));
+				}
 			}
 		}
 		else

corelib/src/Signature.cpp

@@ -39,6 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
+	_stamp(0.0),
 	_weight(0),
 	_saved(false),
 	_modified(true),

corelib/src/resources/DatabaseSchema.sql.in

@@ -44,6 +44,7 @@ CREATE TABLE Link (
 	rot_variance FLOAT NOT NULL,
 	trans_variance FLOAT NOT NULL,
 	transform BLOB,
+	user_data BLOB,           -- compressed data (User data)
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)
 );
@@ -82,7 +83,7 @@ CREATE TABLE Statistics (
 );
 
 CREATE TABLE Admin (
-	version INTEGER,
+	version TEXT,
 	time_enter DATE
 );
 

corelib/src/util2d.cpp

@@ -281,7 +281,10 @@ cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
 		}
 		if(countOverMax)
 		{
-			UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
+			UWARN("Depth conversion error, %d depth values ignored because "
+				  "they are over the maximum depth allowed (65535 mm). Is the depth "
+				  "image really in meters? 32 bits images should be in meters, "
+				  "and 16 bits should be in mm.", countOverMax);
 		}
 	}
 	return depth16U;

corelib/src/util3d_registration.cpp

@@ -233,8 +233,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	est->setInputTarget(cloudA);
+	est->setInputTarget(cloudB);
-	est->setInputSource(cloudB);
+	est->setInputSource(cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 
@@ -266,8 +266,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-	est->setInputTarget(cloudA);
+	est->setInputTarget(cloudB);
-	est->setInputSource(cloudB);
+	est->setInputSource(cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -149,6 +149,8 @@ private slots:
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
+	void sendWaypoints();
+	void postGoal(const QString & goal);
 	void cancelGoal();
 	void label();
 	void downloadAllClouds();
@@ -167,6 +169,7 @@ private slots:
 	void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
 	void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
 	void processRtabmapLabelErrorEvent(int id, const QString & label);
+	void processRtabmapGoalStatusEvent(int status);
 	void changeImgRateSetting();
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
@@ -202,6 +205,7 @@ signals:
 	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
 	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
 	void rtabmapLabelErrorReceived(int id, const QString & label);
+	void rtabmapGoalStatusEventReceived(int status);
 	void imgRateChanged(double);
 	void detectionRateChanged(double);
 	void timeLimitChanged(float);
@@ -277,6 +281,8 @@ private:
 	bool _odomImageShow;
 	bool _odomImageDepthShow;
 	bool _savedMaximized;
+	QStringList _waypoints;
+	int _waypointsIndex;
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -186,6 +186,7 @@ public:
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
+	bool getSourceDatabaseGoalsIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	bool getSourceDatabaseStampsUsed() const;//Database group
 	bool isSourceRGBDColorOnly() const;

guilib/src/DatabaseViewer.cpp

@@ -2221,11 +2221,11 @@ void DatabaseViewer::updateConstraintView(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
 {
-	std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
+	std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
-	if(iter != linksRefined_.end())
+	if(iterLink != linksRefined_.end())
 	{
-		link = iter->second;
+		link = iterLink->second;
 	}
 	rtabmap::Transform t = link.transform();
 
@@ -2428,6 +2428,141 @@ void DatabaseViewer::updateConstraintView(
 			if(ui_->checkBox_show2DScans->isChecked())
 			{
 				//cloud 2d
+
+				ui_->constraintsViewer->removeCloud("scan2");
+				ui_->constraintsViewer->removeGraph("scan2graph");
+				if(link.type() == Link::kLocalSpaceClosure && !link.userDataCompressed().empty())
+				{
+					std::vector<int> ids;
+					cv::Mat userData = link.uncompressUserDataConst();
+					if(userData.type() == CV_8SC1 &&
+					   userData.rows == 1 &&
+					   userData.cols >= 8 && // including null str ending
+					   userData.at<char>(userData.cols-1) == 0 &&
+					   memcmp(userData.data, "SCANS:", 6) == 0)
+					{
+						std::string scansStr = (const char *)userData.data;
+						if(!scansStr.empty())
+						{
+							std::list<std::string> strs = uSplit(scansStr, ':');
+							if(strs.size() == 2)
+							{
+								std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
+								for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
+								{
+									ids.push_back(atoi(iter->c_str()));
+									if(ids.back() == link.from())
+									{
+										ids.pop_back();
+									}
+								}
+							}
+						}
+					}
+					if(ids.size())
+					{
+						//add other scans matching
+						//optimize the path's poses locally
+
+						graph::Optimizer * optimizer = 0;
+						if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
+						{
+							optimizer = new graph::GTSAMOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0,
+									ui_->checkBox_robust->isChecked());
+						}
+						else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
+						{
+							UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0);
+							optimizer = new graph::G2OOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0,
+									ui_->checkBox_robust->isChecked());
+						}
+						else
+						{
+							optimizer = new graph::TOROOptimizer(
+									ui_->spinBox_iterations->value(),
+									ui_->checkBox_2dslam->isChecked(),
+									ui_->checkBox_ignoreCovariance->isChecked(),
+									0.0);
+						}
+						std::map<int, rtabmap::Transform> poses;
+						for(unsigned int i=0; i<ids.size(); ++i)
+						{
+							if(uContains(poses_, ids[i]))
+							{
+								poses.insert(*poses_.find(ids[i]));
+							}
+							else
+							{
+								UERROR("Not found %d node!", ids[i]);
+							}
+						}
+						if(poses.size())
+						{
+							UASSERT(uContains(poses, link.to()));
+							std::map<int, rtabmap::Transform> posesOut;
+							std::multimap<int, rtabmap::Link> linksOut;
+							optimizer->getConnectedGraph(
+									link.to(),
+									poses,
+									updateLinksWithModifications(links_),
+									posesOut,
+									linksOut);
+
+							QTime time;
+							time.start();
+							std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
+							delete optimizer;
+
+							// transform local poses in loop referential
+							Transform u = t * finalPoses.at(link.to()).inverse();
+							pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+							for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
+							{
+								iter->second = u * iter->second;
+								if(iter->first != link.to()) // already added to view
+								{
+									//create scan
+									SensorData data = memory_->getNodeData(iter->first, false);
+									cv::Mat scan;
+									data.uncompressDataConst(0, 0, &scan, 0);
+									if(!scan.empty())
+									{
+										pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
+										if(assembledScans->size() == 0)
+										{
+											assembledScans = util3d::transformPointCloud(scanCloud, iter->second);
+										}
+										else
+										{
+											*assembledScans += *util3d::transformPointCloud(scanCloud, iter->second);
+										}
+									}
+								}
+								graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+							}
+
+							if(assembledScans->size())
+							{
+								ui_->constraintsViewer->addOrUpdateCloud("scan2", assembledScans, Transform::getIdentity(), Qt::cyan);
+							}
+							if(graph->size())
+							{
+								ui_->constraintsViewer->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
+							}
+						}
+					}
+				}
+
+				// Added loop closure scans
 				pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
 				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
 				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
@@ -2453,6 +2588,7 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->removeCloud("scan0");
 				ui_->constraintsViewer->removeCloud("scan1");
+				ui_->constraintsViewer->removeCloud("scan2");
 			}
 		}
 		else
@@ -2473,6 +2609,7 @@ void DatabaseViewer::updateConstraintView(
 			{
 				ui_->constraintsViewer->removeCloud("scan1");
 			}
+			ui_->constraintsViewer->removeCloud("scan2");
 		}
 
 		//update coordinate

guilib/src/MainWindow.cpp

@@ -136,6 +136,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_databaseUpdated(false),
 	_odomImageShow(true),
 	_odomImageDepthShow(false),
+	_savedMaximized(false),
+	_waypointsIndex(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
 	_lastOdomInfoUpdateTime(0),
@@ -255,6 +257,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
 	connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
 	connect(this, SIGNAL(rtabmapLabelErrorReceived(int, const QString &)), this, SLOT(processRtabmapLabelErrorEvent(int, const QString &)));
+	connect(this, SIGNAL(rtabmapGoalStatusEventReceived(int)), this, SLOT(processRtabmapGoalStatusEvent(int)));
+
 	// Dock Widget view actions (Menu->Window)
 	_ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_posterior->toggleViewAction());
@@ -291,6 +295,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
 	connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
 	connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
+	connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
 	connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
 	connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
@@ -662,6 +667,10 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		RtabmapLabelErrorEvent * rtabmapLabelErrorEvent = (RtabmapLabelErrorEvent*)anEvent;
 		emit rtabmapLabelErrorReceived(rtabmapLabelErrorEvent->id(), QString(rtabmapLabelErrorEvent->label().c_str()));
 	}
+	else if(anEvent->getClassName().compare("RtabmapGoalStatusEvent") == 0)
+	{
+		emit rtabmapGoalStatusEventReceived(anEvent->getCode());
+	}
 	else if(anEvent->getClassName().compare("CameraEvent") == 0)
 	{
 		CameraEvent * cameraEvent = (CameraEvent*)anEvent;
@@ -2227,6 +2236,15 @@ void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)
 	warn->show();
 }
 
+void MainWindow::processRtabmapGoalStatusEvent(int status)
+{
+	_ui->widget_console->appendMsg(tr("Goal status received=%1").arg(status), ULogger::kInfo);
+	if(_waypoints.size())
+	{
+		this->postGoal(_waypoints.at(++_waypointsIndex % _waypoints.size()));
+	}
+}
+
 void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 {
 	ULOGGER_DEBUG("");
@@ -2944,7 +2962,8 @@ void MainWindow::startDetection()
 		_dbReader = new DBReader(_preferencesDialog->getSourceDatabasePath().toStdString(),
 								 _preferencesDialog->getSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate(),
 								 _preferencesDialog->getSourceDatabaseOdometryIgnored(),
-								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored());
+								 _preferencesDialog->getSourceDatabaseGoalDelayIgnored(),
+								 _preferencesDialog->getSourceDatabaseGoalsIgnored());
 
 		//Create odometry thread if rgdb slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()) &&
@@ -3902,18 +3921,61 @@ void MainWindow::sendGoal()
 {
 	UINFO("Sending a goal...");
 	bool ok = false;
-	int id = QInputDialog::getInt(this, tr("Send a goal"), tr("Goal location ID: "), 1, 1, 99999, 1, &ok);
+	QString text = QInputDialog::getText(this, tr("Send a goal"), tr("Goal location ID or label: "), QLineEdit::Normal, "", &ok);
+	if(ok && !text.isEmpty())
+	{
+		_waypoints.clear();
+		_waypointsIndex = 0;
+
+		this->postGoal(text);
+	}
+}
+
+void MainWindow::sendWaypoints()
+{
+	UINFO("Sending waypoints...");
+	bool ok = false;
+	QString text = QInputDialog::getText(this, tr("Send waypoints"), tr("Waypoint IDs or labels (separated by spaces): "), QLineEdit::Normal, "", &ok);
+	if(ok && !text.isEmpty())
+	{
+		QStringList wp = text.split(' ');
+		if(wp.size() < 2)
+		{
+			QMessageBox::warning(this, tr("Send waypoints"), tr("At least two waypoints should be set. For only one goal, use send goal action."));
+		}
+		else
+		{
+			_waypoints = wp;
+			_waypointsIndex = 0;
+			this->postGoal(_waypoints.at(_waypointsIndex));
+		}
+	}
+}
+
+void MainWindow::postGoal(const QString & goal)
+{
+	if(!goal.isEmpty())
+	{
+		bool ok = false;
+		int id = goal.toInt(&ok);
+		_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
+		UINFO("Posting event with goal %s", goal.toStdString().c_str());
 		if(ok)
 		{
-		_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
-		UINFO("Posting event with goal %d", id);
 			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, id));
 		}
+		else
+		{
+			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goal.toStdString()));
+		}
+	}
 }
 
 void MainWindow::cancelGoal()
 {
 	UINFO("Cancelling goal...");
+	_waypoints.clear();
+	_waypointsIndex = 0;
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
 }
 

guilib/src/PreferencesDialog.cpp

@@ -361,6 +361,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_database_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreOdometry, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreGoalDelay, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_ignoreGoals, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -585,9 +586,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str());
 
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
-	_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 	_ui->graphPlan_stuckIterations->setObjectName(Parameters::kRGBDPlanStuckIterations().c_str());
+	_ui->graphPlan_linearVelocity->setObjectName(Parameters::kRGBDPlanLinearVelocity().c_str());
+	_ui->graphPlan_angularVelocity->setObjectName(Parameters::kRGBDPlanAngularVelocity().c_str());
 
 	_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
 	_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());
@@ -1091,6 +1093,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->source_checkBox_ignoreOdometry->setChecked(false);
 		_ui->source_checkBox_ignoreGoalDelay->setChecked(false);
+		_ui->source_checkBox_ignoreGoals->setChecked(false);
 		_ui->source_spinBox_databaseStartPos->setValue(0);
 		_ui->source_checkBox_useDbStamps->setChecked(true);
 
@@ -1448,6 +1451,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_database_lineEdit_path->setText(settings.value("path",_ui->source_database_lineEdit_path->text()).toString());
 	_ui->source_checkBox_ignoreOdometry->setChecked(settings.value("ignoreOdometry", _ui->source_checkBox_ignoreOdometry->isChecked()).toBool());
 	_ui->source_checkBox_ignoreGoalDelay->setChecked(settings.value("ignoreGoalDelay", _ui->source_checkBox_ignoreGoalDelay->isChecked()).toBool());
+	_ui->source_checkBox_ignoreGoals->setChecked(settings.value("ignoreGoals", _ui->source_checkBox_ignoreGoals->isChecked()).toBool());
 	_ui->source_spinBox_databaseStartPos->setValue(settings.value("startPos", _ui->source_spinBox_databaseStartPos->value()).toInt());
 	_ui->source_checkBox_useDbStamps->setChecked(settings.value("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked()).toBool());
 	settings.endGroup(); // Database
@@ -1760,6 +1764,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("path", 			   _ui->source_database_lineEdit_path->text());
 	settings.setValue("ignoreOdometry",    _ui->source_checkBox_ignoreOdometry->isChecked());
 	settings.setValue("ignoreGoalDelay",   _ui->source_checkBox_ignoreGoalDelay->isChecked());
+	settings.setValue("ignoreGoals",       _ui->source_checkBox_ignoreGoals->isChecked());
 	settings.setValue("startPos",          _ui->source_spinBox_databaseStartPos->value());
 	settings.setValue("useDatabaseStamps", _ui->source_checkBox_useDbStamps->isChecked());
 	settings.endGroup(); // Database
@@ -3517,6 +3522,10 @@ bool PreferencesDialog::getSourceDatabaseGoalDelayIgnored() const
 {
 	return _ui->source_checkBox_ignoreGoalDelay->isChecked();
 }
+bool PreferencesDialog::getSourceDatabaseGoalsIgnored() const
+{
+	return _ui->source_checkBox_ignoreGoals->isChecked();
+}
 int PreferencesDialog::getSourceDatabaseStartPos() const
 {
 	return _ui->source_spinBox_databaseStartPos->value();

guilib/src/ui/mainWindow.ui

@@ -211,6 +211,7 @@
     <addaction name="actionTrigger_a_new_map"/>
     <addaction name="separator"/>
     <addaction name="actionLabel_current_location"/>
+    <addaction name="actionSend_waypoints"/>
     <addaction name="actionSend_goal"/>
     <addaction name="actionCancel_goal"/>
    </widget>
@@ -1271,6 +1272,11 @@
     <string>Custom...</string>
    </property>
   </action>
+  <action name="actionSend_waypoints">
+   <property name="text">
+    <string>Send waypoints...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-374</y>
-            <width>755</width>
+            <width>760</width>
-            <height>1715</height>
+            <height>1678</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>20</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1769,7 +1769,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>1</number>
+                       <number>3</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2409,7 +2409,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>2</number>
+                              <number>3</number>
                              </property>
                              <widget class="QWidget" name="page_49"/>
                              <widget class="QWidget" name="page_48"/>
@@ -2929,7 +2929,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item row="0" column="1">
                             <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                            </item>
-                           <item row="4" column="1">
+                           <item row="5" column="1">
                             <widget class="QLabel" name="label_58">
                              <property name="text">
                               <string>Start position (index)</string>
@@ -2939,7 +2939,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="4" column="0">
+                           <item row="5" column="0">
                             <widget class="QSpinBox" name="source_spinBox_databaseStartPos">
                              <property name="minimum">
                               <number>0</number>
@@ -2962,7 +2962,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="3" column="1">
+                           <item row="4" column="1">
                             <widget class="QLabel" name="label_80">
                              <property name="text">
                               <string>Ignore goal delay.</string>
@@ -2975,7 +2975,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="3" column="0">
+                           <item row="4" column="0">
                             <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
                              <property name="text">
                               <string/>
@@ -3002,7 +3002,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="5" column="1">
+                           <item row="6" column="1">
                             <spacer name="verticalSpacer_35">
                              <property name="orientation">
                               <enum>Qt::Vertical</enum>
@@ -3015,6 +3015,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </spacer>
                            </item>
+                           <item row="3" column="1">
+                            <widget class="QLabel" name="label_263">
+                             <property name="text">
+                              <string>Ignore goals saved in the database.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="3" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreGoals">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                           </layout>
                          </widget>
                         </item>
@@ -6986,27 +7006,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
-                          <widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="1">
-                          <widget class="QLabel" name="label_space3_4">
-                           <property name="text">
-                            <string>Add virtual links. Before planning in the graph, near nodes are linked together. The maximum distance is defined by &quot;Goal reached radius&quot; above. If &quot;Maximum ID difference&quot; below is set, only close nodes in time can be linked together.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="1">
                           <widget class="QLabel" name="label_space3_5">
                            <property name="text">
                             <string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
@@ -7019,7 +7019,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="2" column="0">
                           <widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
                            <property name="text">
                             <string/>
@@ -7046,6 +7046,58 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="graphPlan_linearVelocity">
+                           <property name="suffix">
+                            <string> m/s</string>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="0">
+                          <widget class="QDoubleSpinBox" name="graphPlan_angularVelocity">
+                           <property name="suffix">
+                            <string> rad/s</string>
+                           </property>
+                           <property name="maximum">
+                            <double>10.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_space3_6">
+                           <property name="text">
+                            <string>Linear velocity used to compute path weights based on time instead of distance (0=disabled).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="1">
+                          <widget class="QLabel" name="label_space3_7">
+                           <property name="text">
+                            <string>Angular velocity used to compute path weights based on time instead of distance. Paths with poses in the same direction of the robot will have lower costs (0=disabled).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>