0.18.3: added landmarks (graph optimization, localization, navigation)

corelib/include/rtabmap/core/DBDriver.h

@@ -167,13 +167,13 @@ public:
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
-	void loadTags(int signatureId, std::map<int, TransformStamped> & tags) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
-	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
 	void getNodeIdByLabel(const std::string & label, int & id) const;
 	void getAllLabels(std::map<int, std::string> & labels) const;
 
@@ -260,16 +260,16 @@ protected:
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
-	virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -131,16 +131,16 @@ protected:
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
-	virtual void loadTagsQuery(int signatureId, std::map<int, TransformStamped> & tags) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
@@ -152,7 +152,6 @@ private:
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
-	std::string queryStepTag() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
 	std::string queryStepOccupancyGridUpdate() const;
@@ -162,7 +161,6 @@ private:
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
-	void stepTag(sqlite3_stmt * ppStmt, int nodeId, int tagId, const TransformStamped & pose) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
@@ -175,7 +173,6 @@ private:
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
-	void loadTagsQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
 protected:

corelib/include/rtabmap/core/Landmark.h

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2018, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap {
+
+class Landmark
+{
+public:
+	Landmark() :
+		id_(0)
+	{}
+	Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
+		id_(id),
+		pose_(pose),
+		covariance_(covariance)
+	{
+		UASSERT(id_>0);
+		UASSERT(!pose_.isNull());
+		UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(0,0)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(1,1)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(2,2)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(3,3)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(4,4)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 1 if unknown).", covariance_.at<double>(5,5)).c_str());
+	}
+
+	virtual ~Landmark() {}
+
+	const int & id() const {return id_;}
+	const Transform & pose() const {return pose_;}
+	const cv::Mat & covariance() const {return covariance_;}
+
+private:
+	int id_;
+	Transform pose_;
+	cv::Mat covariance_;
+};
+
+typedef std::map<int, Landmark> Landmarks;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

corelib/include/rtabmap/core/Link.h

@@ -46,7 +46,11 @@ public:
 		kUserClosure,
 		kVirtualClosure,
 		kNeighborMerged,
-		kPosePrior,
+		kPosePrior, // Absolute pose in /world frame, From == To
+		kLandmark,  // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To"
+		kEnd,
+		kAllWithLandmarks = 98,
+		kAllWithoutLandmarks = 99,
 		kUndef = 99};
 	Link();
 	Link(int from,
@@ -56,7 +60,7 @@ public:
 			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
 			const cv::Mat & userData = cv::Mat());
 
-	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
+	bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
 
 	int from() const {return from_;}
 	int to() const {return to_;}

corelib/include/rtabmap/core/Memory.h

@@ -150,17 +150,21 @@ public:
 			bool lookInDatabase = false) const;
 	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	std::map<int, Link> getLinks(int signatureId,
-			bool lookInDatabase = false) const;
-	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
+	std::map<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
+			bool lookInDatabase = false,
+			bool withLandmarks = false) const;
+	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
 	const Signature * getLastWorkingSignature() const;
+	std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);
 	const std::map<int, std::string> & getAllLabels() const {return _labels;}
+	const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
+	const std::map<int, std::set<int> > & getLandmarksInvertedIndex() const {return _landmarksInvertedIndex;}
 	bool allNodesInWM() const {return _allNodesInWM;}
 
 	/**
@@ -229,7 +233,8 @@ public:
 			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
-			bool lookInDatabase = false);
+			bool lookInDatabase = false,
+			bool landmarksAdded = false);
 
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
@@ -329,6 +334,8 @@ private:
 	std::map<int, double> _workingMem; // id,age
 	std::map<int, Transform> _groundTruths;
 	std::map<int, std::string> _labels;
+	std::map<int, std::set<int> > _landmarksIndex;         // <nodeId, landmarkIds>
+	std::map<int, std::set<int> > _landmarksInvertedIndex; // <landmarkId, nodeIds>
 
 	//Keypoint stuff
 	VWDictionary * _vwd;

corelib/include/rtabmap/core/Optimizer.h

@@ -89,6 +89,7 @@ public:
 	double epsilon() const {return epsilon_;}
 	bool isRobust() const {return robust_;}
 	bool priorsIgnored() const {return priorsIgnored_;}
+	bool landmarksIgnored() const {return landmarksIgnored_;}
 
 	// setters
 	void setIterations(int iterations) {iterations_ = iterations;}
@@ -97,6 +98,7 @@ public:
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
 	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
+	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -172,7 +174,8 @@ protected:
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust(),
-			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored());
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored(),
+			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -182,6 +185,7 @@ private:
 	double epsilon_;
 	bool robust_;
 	bool priorsIgnored_;
+	bool landmarksIgnored_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -385,6 +385,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
     RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
+    RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
 
 #ifdef RTABMAP_ORB_SLAM2
     RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");

corelib/include/rtabmap/core/Rtabmap.h

@@ -189,7 +189,7 @@ public:
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
-	std::map<int, std::map<int, Transform> > getPaths(std::map<int, Transform> poses, const Transform & target, int maxGraphDepth = 0) const;
+	std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;

corelib/include/rtabmap/core/SensorData.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/IMU.h>
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/EnvSensor.h>
+#include <rtabmap/core/Landmark.h>
 
 namespace rtabmap
 {
@@ -246,6 +247,9 @@ public:
 	void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
 	const EnvSensors & envSensors() const {return _envSensors;}
 
+	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
+	const Landmarks & landmarks() const {return _landmarks;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
 	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
 
@@ -283,8 +287,8 @@ private:
 	// environmental sensors
 	EnvSensors _envSensors;
 
-	// tags
-	std::map<int, TransformStamped> _tags;
+	// landmarks
+	Landmarks _landmarks;
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;

corelib/include/rtabmap/core/Signature.h

@@ -90,9 +90,8 @@ public:
 	void removeLink(int idTo);
 	void removeVirtualLinks();
 
-	void setTags(const std::map<int, TransformStamped> & tags) {_tags = tags;}
-	void addTag(int id, const TransformStamped & pose) {_tags.insert(std::make_pair(id, pose));}
-	const std::map<int, TransformStamped> & getTags() const {return _tags;}
+	void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
+	const std::map<int, Link> & getLandmarks() const {return _landmarks;}
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -145,7 +144,7 @@ private:
 	int _mapId;
 	double _stamp;
 	std::map<int, Link> _links; // id, transform
-	std::map<int, TransformStamped> _tags;
+	std::map<int, Link> _landmarks;
 	int _weight;
 	std::string _label;
 	bool _saved; // If it's saved to bd

corelib/include/rtabmap/core/Statistics.h

@@ -69,6 +69,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Optimization_iterations, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
+	RTABMAP_STATS(Loop, Landmark_detected,);
+	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
 
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);