Version 0.8.8: added "user_data" field in database. Added WifiMapping example. Fixed FATAL error when initializing rtabmap with no database.

CMakeLists.txt

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

corelib/include/rtabmap/core/DBDriver.h

@@ -97,7 +97,7 @@ public:
 	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
 	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
 	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
@@ -136,7 +136,7 @@ private:
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;

corelib/include/rtabmap/core/Memory.h

@@ -110,6 +110,7 @@ public:
 	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
 	bool labelSignature(int id, const std::string & label);
 	std::map<int, std::string> getAllLabels() const;
+	bool setUserData(int id, const std::vector<unsigned char> & data);
 	int getDatabaseMemoryUsed() const; // in bytes
 	double getDbSavingTime() const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
@@ -119,6 +120,7 @@ public:
 			int & weight,
 			std::string & label,
 			double & stamp,
+			std::vector<unsigned char> & userData,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	Signature getSignatureData(int locationId, bool uncompressedData = false);

corelib/include/rtabmap/core/Rtabmap.h

@@ -103,6 +103,7 @@ public:
 
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
+	bool setUserData(int id, const std::vector<unsigned char> & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
 	void generateTOROGraph(const std::string & path, bool optimized, bool global);
 	void resetMemory();

corelib/include/rtabmap/core/RtabmapThread.h

@@ -69,7 +69,8 @@ public:
 		kStatePublishingMapGlobal,
 		kStatePublishingTOROGraphLocal,
 		kStatePublishingTOROGraphGlobal,
-		kStateTriggeringMap
+		kStateTriggeringMap,
+		kStateAddingUserData
 	};
 
 public:
@@ -112,6 +113,9 @@ private:
 	Transform lastPose_;
 	float _rotVariance;
 	float _transVariance;
+
+	std::vector<unsigned char> _userData;
+	UMutex _userDataMutex;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/SensorData.h

@@ -43,7 +43,7 @@ class RTABMAP_EXP SensorData
 {
 public:
 	SensorData(); // empty constructor
-	SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0);
+	SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	// Metric constructor
 	SensorData(const cv::Mat & image,
@@ -57,7 +57,8 @@ public:
 		  float poseRotVariance,
 		  float poseTransVariance,
 		  int id,
-		  double stamp);
+		  double stamp,
+		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	// Metric constructor + 2d laser scan
 	SensorData(const cv::Mat & laserScan,
@@ -72,7 +73,8 @@ public:
 		  float poseRotVariance,
 		  float poseTransVariance,
 		  int id,
-		  double stamp);
+		  double stamp,
+		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	virtual ~SensorData() {}
 
@@ -111,6 +113,9 @@ public:
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
+	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
+	const std::vector<unsigned char> userData() const {return _userData;}
+
 private:
 	cv::Mat _image;
 	int _id;
@@ -131,6 +136,9 @@ private:
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;
+
+	// user data
+	std::vector<unsigned char> _userData;
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -60,6 +60,7 @@ public:
 			const std::multimap<int, cv::KeyPoint> & words,
 			const std::multimap<int, pcl::PointXYZ> & words3,
 			const Transform & pose = Transform(),
+			const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
 			const cv::Mat & laserScan = cv::Mat(),
 			const cv::Mat & image = cv::Mat(),
 			const cv::Mat & depth = cv::Mat(),
@@ -85,6 +86,9 @@ public:
 	void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
 	const std::string & getLabel() const {return _label;}
 
+	void setUserData(const std::vector<unsigned char> & data);
+	const std::vector<unsigned char> & getUserData() const {return _userData;}
+
 	double getStamp() const {return _stamp;}
 
 	void addLinks(const std::list<Link> & links);
@@ -153,6 +157,7 @@ private:
 	std::map<int, Link> _links; // id, transform
 	int _weight;
 	std::string _label;
+	std::vector<unsigned char> _userData;
 	bool _saved; // If it's saved to bd
 	bool _modified;
 	bool _linksModified; // Optimization when updating signatures in database

corelib/include/rtabmap/core/Statistics.h

@@ -129,6 +129,8 @@ public:
 
 	void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
 	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
+	void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
+	void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
 	void setSignature(const Signature & s) {_signature = s;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
@@ -149,6 +151,8 @@ public:
 
 	const std::map<int, int> & getMapIds() const {return _mapIds;}
 	const std::map<int, std::string> & getLabels() const {return _labels;}
+	const std::map<int, double> & getStamps() const {return _stamps;}
+	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
 	const Signature & getSignature() const {return _signature;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
@@ -173,6 +177,8 @@ private:
 	// extended data start here...
 	std::map<int, int> _mapIds;
 	std::map<int, std::string> _labels;
+	std::map<int, double> _stamps;
+	std::map<int, std::vector<unsigned char> > _userDatas;
 
 	// Signature data
 	Signature _signature;

corelib/include/rtabmap/core/UserDataEvent.h

@@ -0,0 +1,59 @@
+/*
+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.
+*/
+
+#ifndef USERDATAEVENT_H_
+#define USERDATAEVENT_H_
+
+#include <opencv2/opencv.hpp>
+#include <rtabmap/utilite/UEvent.h>
+
+namespace rtabmap
+{
+
+/**
+ * The user data event.
+ */
+class UserDataEvent : public UEvent
+{
+public:
+	UserDataEvent(const std::vector<unsigned char> & data) :
+		UEvent(0),
+		data_(data)
+	{}
+	~UserDataEvent() {}
+	virtual std::string getClassName() const {return "UserDataEvent";}
+
+	const std::vector<unsigned char> & data() const {return data_;}
+
+private:
+	std::vector<unsigned char> data_;
+};
+
+}
+
+#endif /* USERDATAEVENT_H_ */
+

corelib/src/DBDriver.cpp

@@ -474,7 +474,13 @@ void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
 	}
 }
 
-bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const
+bool DBDriver::getNodeInfo(int signatureId,
+		Transform & pose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp,
+		std::vector<unsigned char> & userData) const
 {
 	bool found = false;
 	// look in the trash
@@ -486,6 +492,7 @@ bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int &
 		weight = _trashSignatures.at(signatureId)->getWeight();
 		label = _trashSignatures.at(signatureId)->getLabel();
 		stamp = _trashSignatures.at(signatureId)->getStamp();
+		userData = _trashSignatures.at(signatureId)->getUserData();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -493,7 +500,7 @@ bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int &
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;

corelib/src/DBDriverSqlite3.cpp

@@ -458,10 +458,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			if(uStrNumCmp(_version, "0.7.0") < 0)
+			if(uStrNumCmp(_version, "0.7.0") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -471,7 +471,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -598,10 +598,10 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.7.0") < 0)
+		if(uStrNumCmp(_version, "0.7.0") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -611,7 +611,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -756,7 +756,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		int & mapId,
 		int & weight,
 		std::string & label,
-		double & stamp) const
+		double & stamp,
+		std::vector<unsigned char> & userData) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -766,9 +767,16 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		std::stringstream query;
 
 		// Prepare the query... Get the map from signature and visual words
-		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
 		{
-			query << "SELECT pose, map_id, weight, label "
+			query << "SELECT pose, map_id, weight, label, stamp, user_data"
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			query << "SELECT pose, map_id, weight, label, stamp "
 					 "FROM Node "
 					 "WHERE id = " << signatureId <<
 					 ";";
@@ -810,6 +818,19 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 				{
 					label = reinterpret_cast<const char*>(p); // label
 				}
+				stamp = sqlite3_column_double(ppStmt, index++); // stamp
+			}
+
+			if(uStrNumCmp(_version, "0.8.8") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
+
+				if(dataSize && data)
+				{
+					userData.resize(dataSize);
+					memcpy(userData.data(), data, dataSize);
+				}
 			}
 
 			rc = sqlite3_step(ppStmt); // next result...
@@ -1073,7 +1094,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		unsigned int loaded = 0;
 
 		// Load nodes information
-		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
+		{
+			query << "SELECT id, map_id, weight, pose, stamp, label, user_data "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
 		{
 			query << "SELECT id, map_id, weight, pose, stamp, label "
 				  << "FROM Node "
@@ -1104,6 +1131,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			const void * data = 0;
 			int dataSize = 0;
 			std::string label;
+			std::vector<unsigned char> userData;
 
 			// Process the result if one
 			rc = sqlite3_step(ppStmt);
@@ -1123,14 +1151,26 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 				if(uStrNumCmp(_version, "0.8.5") >= 0)
 				{
-					stamp = sqlite3_column_double(ppStmt, index++);
+					stamp = sqlite3_column_double(ppStmt, index++); // stamp
-					const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+					const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
 					if(p)
 					{
 						label = reinterpret_cast<const char*>(p);
 					}
 				}
 
+				if(uStrNumCmp(_version, "0.8.8") >= 0)
+				{
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
+
+					if(dataSize && data)
+					{
+						userData.resize(dataSize);
+						memcpy(userData.data(), data, dataSize);
+					}
+				}
+
 				rc = sqlite3_step(ppStmt);
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1147,7 +1187,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						label,
 						std::multimap<int, cv::KeyPoint>(),
 						std::multimap<int, pcl::PointXYZ>(),
-						pose);
+						pose,
+						userData);
 				s->setSaved(true);
 				nodes.push_back(s);
 				++loaded;
@@ -1704,7 +1745,18 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 		Signature * s = 0;
 
 		std::string query;
-		if(uStrNumCmp(_version, "0.8.5") >= 0)
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
+		{
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, label=?, user_data=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=?, label=?, user_data=? WHERE id=?;";
+			}
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
 		{
 			if(updateTimestamp)
 			{
@@ -1752,6 +1804,20 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 					}
 				}
 
+				if(uStrNumCmp(_version, "0.8.8") >= 0)
+				{
+					if(s->getUserData().empty())
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+					else
+					{
+						rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+				}
+
 				rc = sqlite3_bind_int(ppStmt, index++, s->id());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -2049,7 +2115,11 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 
 std::string DBDriverSqlite3::queryStepNode() const
 {
-	if(uStrNumCmp(_version, "0.8.5") >= 0)
+	if(uStrNumCmp(_version, "0.8.8") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.8.5") >= 0)
 	{
 		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
 	}
@@ -2091,6 +2161,20 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 		}
 	}
 
+	if(uStrNumCmp(_version, "0.8.8") >= 0)
+	{
+		if(s->getUserData().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2139,13 +2223,13 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
-	if(uStrNumCmp(_version, "0.7.0") < 0)
+	if(uStrNumCmp(_version, "0.7.0") >= 0)
 	{
-		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
 	}
 	else
 	{
-		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
+		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
 	}
 }
 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
@@ -2180,12 +2264,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(uStrNumCmp(_version, "0.7.0") < 0)
+	if(uStrNumCmp(_version, "0.7.0") >= 0)
-	{
-		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	}
-	else
 	{
 		rc = sqlite3_bind_double(ppStmt, index++, fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2196,6 +2275,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		rc = sqlite3_bind_double(ppStmt, index++, cy);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
+	else
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
 
 	rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

@@ -82,7 +82,7 @@ private:
 			float & cy,
 			Transform & localTransform) const;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;

corelib/src/DBReader.cpp

@@ -195,13 +195,17 @@ SensorData DBReader::getNextData()
 			Transform localTransform, pose;
 			float rotVariance = 1.0f;
 			float transVariance = 1.0f;
+			std::vector<unsigned char> userData;
 			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
+
+			// info
+			int weight;
+			std::string label;
+			double stamp;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+
 			if(!_odometryIgnored)
 			{
-				int weight;
-				std::string label;
-				double stamp;
-				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
 				std::map<int, Link> links;
 				_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
 				if(links.size())
@@ -211,6 +215,11 @@ SensorData DBReader::getNextData()
 					transVariance = links.begin()->second.transVariance();
 				}
 			}
+			else
+			{
+				pose.setNull();
+			}
+
 			int seq = *_currentId;
 			++_currentId;
 			if(imageBytes.empty())
@@ -237,7 +246,8 @@ SensorData DBReader::getNextData()
 					rotVariance,
 					transVariance,
 					seq,
-					UTimer::now());
+					UTimer::now(),
+					userData);
 			UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
 					data.laserScan().empty()?0:1,
 					data.image().empty()?0:1,

corelib/src/Memory.cpp

@@ -1648,41 +1648,38 @@ int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase
 
 bool Memory::labelSignature(int id, const std::string & label)
 {
-	if(!label.empty())
+	// verify that this label is not used
+	int idFound=getSignatureIdByLabel(label);
+	if(idFound == 0 || idFound == id)
 	{
-		// verify that this label is not used
+		Signature * s  = this->_getSignature(id);
-		int idFound=getSignatureIdByLabel(label);
+		if(s)
-		if(idFound == 0 || idFound == id)
 		{
-			Signature * s  = this->_getSignature(id);
+			s->setLabel(label);
-			if(s)
+			return true;
+		}
+		else if(_dbDriver)
+		{
+			std::list<int> ids;
+			ids.push_back(id);
+			std::list<Signature *> signatures;
+			_dbDriver->loadSignatures(ids,signatures);
+			if(signatures.size())
 			{
-				s->setLabel(label);
+				signatures.front()->setLabel(label);
+				_dbDriver->asyncSave(signatures.front()); // move it again to trash
 				return true;
 			}
-			else if(_dbDriver)
-			{
-				std::list<int> ids;
-				ids.push_back(id);
-				std::list<Signature *> signatures;
-				_dbDriver->loadSignatures(ids,signatures);
-				if(signatures.size())
-				{
-					signatures.front()->setLabel(label);
-					_dbDriver->asyncSave(signatures.front()); // move it again to trash
-					return true;
-				}
-			}
-			else
-			{
-				UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
-			}
 		}
-		else if(idFound)
+		else
 		{
-			UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+			UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
 		}
 	}
+	else if(idFound)
+	{
+		UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+	}
 	return false;
 }
 
@@ -1703,6 +1700,34 @@ std::map<int, std::string> Memory::getAllLabels() const
 	return labels;
 }
 
+bool Memory::setUserData(int id, const std::vector<unsigned char> & data)
+{
+	Signature * s  = this->_getSignature(id);
+	if(s)
+	{
+		s->setUserData(data);
+		return true;
+	}
+	else if(_dbDriver)
+	{
+		std::list<int> ids;
+		ids.push_back(id);
+		std::list<Signature *> signatures;
+		_dbDriver->loadSignatures(ids,signatures);
+		if(signatures.size())
+		{
+			signatures.front()->setUserData(data);
+			_dbDriver->asyncSave(signatures.front()); // move it again to trash
+			return true;
+		}
+	}
+	else
+	{
+		UERROR("Node %d not found, failed to set user data (size=%d)!", id, data.size());
+	}
+	return false;
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -2711,7 +2736,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 	int mapId, weight;
 	std::string label;
 	double stamp;
-	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase);
+	std::vector<unsigned char> userData;
+	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, userData, lookInDatabase);
 	return pose;
 }
 
@@ -2721,6 +2747,7 @@ bool Memory::getNodeInfo(int signatureId,
 		int & weight,
 		std::string & label,
 		double & stamp,
+		std::vector<unsigned char> & userData,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -2731,11 +2758,12 @@ bool Memory::getNodeInfo(int signatureId,
 		weight = s->getWeight();
 		label = s->getLabel();
 		stamp = s->getStamp();
+		userData = s->getUserData();
 		return true;
 	}
 	else if(lookInDatabase && _dbDriver)
 	{
-		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp);
+		return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, userData);
 	}
 	return false;
 }
@@ -3658,6 +3686,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			words,
 			words3D,
 			data.pose(),
+			data.userData(),
 			ctDepth2d.getCompressedData(),
 			ctImage.getCompressedData(),
 			ctDepth.getCompressedData(),
@@ -3677,6 +3706,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			words,
 			words3D,
 			data.pose(),
+			data.userData(),
 			rtabmap::compressData2(data.laserScan()));
 	}
 	if(this->isRawDataKept())

corelib/src/Rtabmap.cpp

@@ -261,15 +261,32 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	}
 
 	_databasePath = databasePath;
+	if(!_databasePath.empty())
+	{
+		UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
+		UINFO("Using database \"%s\".", _databasePath.c_str());
+	}
+	else
+	{
+		UWARN("Using empty database. Mapping session will not be saved.");
+	}
+
+	bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
+
+	// If not exist, create a memory
+	if(!_memory)
+	{
+		_memory = new Memory(parameters);
+		_memory->init(_databasePath, false, parameters, true);
+	}
+
+	// Parse all parameters
+	this->parseParameters(parameters);
+
 	if(_databasePath.empty())
 	{
-		_databasePath = _wDir + "/" + Parameters::getDefaultDatabaseName();
+		_statisticLogged = false;
 	}
-	UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
-
-	UINFO("Using database \"%s\".", _databasePath.c_str());
-	bool newDatabase = !UFile::exists(_databasePath);
-	this->parseParameters(parameters);
 	setupLogFiles(newDatabase);
 }
 
@@ -417,16 +434,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	{
 		_graphOptimizer->parseParameters(parameters);
 	}
-
-	if(!_memory)
-	{
-		if(!_databasePath.empty())
-		{
-			_memory = new Memory(parameters);
-			_memory->init(_databasePath, false, parameters, true);
-		}
-	}
 	else
+	{
+		optimizerType = (graph::Optimizer::Type)Parameters::defaultRGBDOptimizeStrategy();
+		_graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
+	}
+
+	if(_memory)
 	{
 		_memory->parseParameters(parameters);
 	}
@@ -621,7 +635,7 @@ int Rtabmap::triggerNewMap()
 
 bool Rtabmap::labelLocation(int id, const std::string & label)
 {
-	if(!label.empty() && _memory)
+	if(_memory)
 	{
 		if(id > 0)
 		{
@@ -639,6 +653,26 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
 	return false;
 }
 
+bool Rtabmap::setUserData(int id, const std::vector<unsigned char> & data)
+{
+	if(_memory)
+	{
+		if(id > 0)
+		{
+			return _memory->setUserData(id, data);
+		}
+		else if(_memory->getLastWorkingSignature())
+		{
+			return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
+		}
+		else
+		{
+			UERROR("Last signature is null! Cannot set user data!");
+		}
+	}
+	return false;
+}
+
 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
 {
 	if(_memory)
@@ -792,10 +826,9 @@ bool Rtabmap::process(const SensorData & data)
 	timer.start();
 	timerTotal.start();
 
-	if(!_memory || !_bayesFilter || !_graphOptimizer)
+	UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
-	{
+	UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
-		UFATAL("RTAB-Map is not initialized, data received is ignored.");
+	UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
-	}
 
 	//============================================================
 	// If RGBD SLAM is enabled, a pose must be set.
@@ -1684,6 +1717,8 @@ bool Rtabmap::process(const SensorData & data)
 				std::map<int, Transform> poses;
 				std::map<int, int> mapIds;
 				std::map<int, std::string> labels;
+				std::map<int, double> stamps;
+				std::map<int, std::vector<unsigned char> > userDatas;
 				std::multimap<int, Link> constraints;
 				_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
 				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -1693,14 +1728,22 @@ bool Rtabmap::process(const SensorData & data)
 					int mapId = -1;
 					std::string label;
 					double stamp = 0;
-					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false);
+					std::vector<unsigned char> userData;
+					_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
 					mapIds.insert(std::make_pair(iter->first, mapId));
 					labels.insert(std::make_pair(iter->first, label));
+					stamps.insert(std::make_pair(iter->first, stamp));
+					if(userData.size())
+					{
+						userDatas.insert(std::make_pair(iter->first, userData));
+					}
 				}
 				statistics_.setPoses(poses);
 				statistics_.setConstraints(constraints);
 				statistics_.setMapIds(mapIds);
 				statistics_.setLabels(labels);
+				statistics_.setStamps(stamps);
+				statistics_.setUserDatas(userDatas);
 			}
 			else // RGBD-SLAM mode
 			{
@@ -1875,6 +1918,8 @@ bool Rtabmap::process(const SensorData & data)
 		{
 			std::map<int, int> mapIds;
 			std::map<int, std::string> labels;
+			std::map<int, double> stamps;
+			std::map<int, std::vector<unsigned char> > userDatas;
 			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
 				Transform odomPose;
@@ -1882,14 +1927,22 @@ bool Rtabmap::process(const SensorData & data)
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+				std::vector<unsigned char> userData;
+				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
 				mapIds.insert(std::make_pair(iter->first, mapId));
 				labels.insert(std::make_pair(iter->first, label));
+				stamps.insert(std::make_pair(iter->first, stamp));
+				if(userData.size())
+				{
+					userDatas.insert(std::make_pair(iter->first, userData));
+				}
 			}
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapIds(mapIds);
 			statistics_.setLabels(labels);
+			statistics_.setStamps(stamps);
+			statistics_.setUserDatas(userDatas);
 		}
 
 	}
@@ -2361,7 +2414,8 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
 			mapIds.insert(std::make_pair(iter->first, mapId));
 			labels.insert(std::make_pair(iter->first, label));
 		}
@@ -2434,7 +2488,8 @@ void Rtabmap::getGraph(
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
+			std::vector<unsigned char> userData;
+			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
 			mapIds.insert(std::make_pair(iter->first, mapId));
 			labels.insert(std::make_pair(iter->first, label));
 		}

corelib/src/RtabmapThread.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/UserDataEvent.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -90,6 +91,12 @@ void RtabmapThread::clearBufferedData()
 		_transVariance = 0;
 	}
 	_dataMutex.unlock();
+
+	_userDataMutex.lock();
+	{
+		_userData = cv::Mat();
+	}
+	_userDataMutex.unlock();
 }
 
 void RtabmapThread::setDetectorRate(float rate)
@@ -175,6 +182,7 @@ void RtabmapThread::mainLoop()
 	}
 	_stateMutex.unlock();
 
+	std::vector<unsigned char> userData;
 	switch(state)
 	{
 	case kStateDetecting:
@@ -243,6 +251,15 @@ void RtabmapThread::mainLoop()
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
 		break;
+	case kStateAddingUserData:
+		_userDataMutex.lock();
+		{
+			userData = _userData;
+			_userData.clear();
+		}
+		_userDataMutex.unlock();
+		_rtabmap->setUserData(0, userData);
+		break;
 	default:
 		UFATAL("Invalid state !?!?");
 		break;
@@ -274,6 +291,32 @@ void RtabmapThread::handleEvent(UEvent* event)
 			lastPose_.setNull();
 		}
 	}
+	else if(event->getClassName().compare("UserDataEvent") == 0)
+	{
+		if(!_paused)
+		{
+			UDEBUG("UserDataEvent");
+			bool updated = false;
+			UserDataEvent * e = (UserDataEvent*)event;
+			_userDataMutex.lock();
+			if(!e->data().empty())
+			{
+				updated = !_userData.empty();
+				_userData = e->data();
+			}
+			_userDataMutex.unlock();
+			if(updated)
+			{
+				UWARN("New user data received before the last one was processed... replacing "
+					"user data with this new one. Note that UserDataEvent should be used only "
+					"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
+			}
+			else
+			{
+				pushNewState(kStateAddingUserData);
+			}
+		}
+	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 	{
 		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;

corelib/src/SensorData.cpp

@@ -36,7 +36,6 @@ namespace rtabmap
  * An id is automatically generated if id=0.
  */
 SensorData::SensorData() :
-	_image(cv::Mat()),
 	_id(0),
 	_stamp(0.0),
 	_fx(0.0f),
@@ -51,7 +50,8 @@ SensorData::SensorData() :
 
 SensorData::SensorData(const cv::Mat & image,
 	  int id,
-	  double stamp) :
+	  double stamp,
+	  const std::vector<unsigned char> & userData) :
 	_image(image),
 	_id(id),
 	_stamp(stamp),
@@ -61,7 +61,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity()),
 	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f)
+	_poseTransVariance(1.0f),
+	_userData(userData)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -79,7 +80,8 @@ SensorData::SensorData(const cv::Mat & image,
 		  float poseRotVariance,
 		  float poseTransVariance,
 		  int id,
-		  double stamp) :
+		  double stamp,
+		  const std::vector<unsigned char> & userData) :
 	_image(image),
 	_id(id),
 	_stamp(stamp),
@@ -91,7 +93,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_pose(pose),
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance)
+	_poseTransVariance(poseTransVariance),
+	_userData(userData)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -115,7 +118,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
 		  float poseRotVariance,
 		  float poseTransVariance,
 		  int id,
-		  double stamp) :
+		  double stamp,
+		  const std::vector<unsigned char> & userData) :
 	_image(image),
 	_id(id),
 	_stamp(stamp),
@@ -128,7 +132,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
 	_pose(pose),
 	_localTransform(localTransform),
 	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance)
+	_poseTransVariance(poseTransVariance),
+	_userData(userData)
 {
 	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
 	UASSERT(image.type() == CV_8UC1 || // Mono

corelib/src/Signature.cpp

@@ -61,6 +61,7 @@ Signature::Signature(
 		const std::multimap<int, cv::KeyPoint> & words,
 		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
+		const std::vector<unsigned char> & userData,
 		const cv::Mat & laserScanCompressed, // in base_link frame
 		const cv::Mat & imageCompressed, // in camera_link frame
 		const cv::Mat & depthCompressed, // in camera_link frame
@@ -74,6 +75,7 @@ Signature::Signature(
 	_stamp(stamp),
 	_weight(weight),
 	_label(label),
+	_userData(userData),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
@@ -97,6 +99,18 @@ Signature::~Signature()
 	//UDEBUG("id=%d", _id);
 }
 
+void Signature::setUserData(const std::vector<unsigned char> & data)
+{
+	if(!_userData.empty() && !data.empty())
+	{
+		UWARN("Node %d: Current user data (%d bytes) overwritten by new data (%d bytes)",
+				_id, (int)_userData.size(), (int)data.size());
+	}
+
+	_modified = true;
+	_userData = data;
+}
+
 void Signature::addLinks(const std::list<Link> & links)
 {
 	for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
@@ -265,7 +279,8 @@ SensorData Signature::toSensorData()
 			rotVariance,
 			transVariance,
 			_id,
-			_stamp);
+			_stamp,
+			_userData);
 }
 
 void Signature::uncompressData()

corelib/src/resources/DatabaseSchema.sql.in

@@ -20,26 +20,28 @@ CREATE TABLE Node (
 	stamp FLOAT,
 	pose BLOB,
 	label TEXT,
+	user_data BLOB,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
 
 CREATE TABLE Image (
 	id INTEGER NOT NULL,
-	data BLOB,
+	data BLOB,       -- compressed image (RGB)
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
 
+-- TODO: Merge "Image" and "Depth" tables to "Data" table.
 CREATE TABLE Depth (
 	id INTEGER NOT NULL,
-	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
+	data BLOB,       -- compressed image (Depth or Right image)
 	fx FLOAT,
-	fy FLOAT,
+	fy FLOAT,        -- baseline if stereo
 	cx FLOAT,
 	cy FLOAT,
 	local_transform BLOB,
-	data2d BLOB,   -- CV_32FC2, Example: Laser scan
+	data2d BLOB,     -- compressed data (Laser scan)
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

examples/CMakeLists.txt

@@ -2,9 +2,15 @@
 ADD_SUBDIRECTORY( BOWMapping )
 
 IF(TARGET rtabmap_gui)
-	ADD_SUBDIRECTORY( RGBDMapping )
+   ADD_SUBDIRECTORY( RGBDMapping )
+	
+   # Only on Linux
+   IF(NOT WIN32 AND NOT APPLE)
+      ADD_SUBDIRECTORY( WifiMapping )
+   ENDIF(NOT WIN32 AND NOT APPLE)
+
 ELSE()
-   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping example will not be built...")
+   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
 ENDIF()
 
 

examples/RGBDMapping/MapBuilder.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QVBoxLayout>
 #include <QtCore/QMetaType>
+#include <QAction>
 
 #ifndef Q_MOC_RUN // Mac OS X issue
 #include "rtabmap/gui/CloudViewer.h"
@@ -41,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/CameraThread.h"
 
 using namespace rtabmap;
 
@@ -49,9 +51,12 @@ class MapBuilder : public QWidget, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	MapBuilder() :
+	//Camera ownership is not transferred!
-		_processingStatistics(false),
+	MapBuilder(CameraThread * camera = 0) :
-		_lastOdometryProcessed(true)
+		camera_(camera),
+		odometryCorrection_(Transform::getIdentity()),
+		processingStatistics_(false),
+		lastOdometryProcessed_(true)
 	{
 		this->setWindowFlags(Qt::Dialog);
 		this->setWindowTitle(tr("3D Map"));
@@ -66,6 +71,11 @@ public:
 
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
 		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
 	}
 
 	virtual ~MapBuilder()
@@ -73,8 +83,24 @@ public:
 		this->unregisterFromEventsManager();
 	}
 
-private slots:
+protected slots:
-	void processOdometry(const rtabmap::SensorData & data)
+	virtual void pauseDetection()
+	{
+		UWARN("");
+		if(camera_)
+		{
+			if(camera_->isCapturing())
+			{
+				camera_->join(true);
+			}
+			else
+			{
+				camera_->start();
+			}
+		}
+	}
+
+	virtual void processOdometry(const rtabmap::SensorData & data)
 	{
 		if(!this->isVisible())
 		{
@@ -120,7 +146,7 @@ private slots:
 						cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
 					}
 				}
-				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, pose))
+				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
 				{
 					UERROR("Adding cloudOdom to viewer failed!");
 				}
@@ -129,19 +155,22 @@ private slots:
 			if(!data.pose().isNull())
 			{
 				// update camera position
-				cloudViewer_->updateCameraTargetPosition(data.pose());
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*data.pose());
 			}
 		}
 		cloudViewer_->update();
 
-		_lastOdometryProcessed = true;
+		lastOdometryProcessed_ = true;
 	}
 
 
-	void processStatistics(const rtabmap::Statistics & stats)
+	virtual void processStatistics(const rtabmap::Statistics & stats)
 	{
-		_processingStatistics = true;
+		processingStatistics_ = true;
 
+		//============================
+		// Add RGB-D clouds
+		//============================
 		const std::map<int, Transform> & poses = stats.poses();
 		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -178,7 +207,7 @@ private slots:
 							s.getDepthCy(),
 							s.getDepthFx(),
 							s.getDepthFy(),
-						   8); // decimation
+						   4); // decimation
 
 					if(cloud->size())
 					{
@@ -196,12 +225,36 @@ private slots:
 			}
 		}
 
+		//============================
+		// Add 3D graph (show all poses)
+		//============================
+		cloudViewer_->removeAllGraphs();
+		cloudViewer_->removeCloud("graph_nodes");
+		if(poses.size())
+		{
+			// Set graph
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+			}
+			*graphNodes = *graph;
+
+
+			// add graph
+			cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
+			cloudViewer_->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+			cloudViewer_->setCloudPointSize("graph_nodes", 5);
+		}
+
+		odometryCorrection_ = stats.mapCorrection();
+
 		cloudViewer_->update();
 
-		_processingStatistics = false;
+		processingStatistics_ = false;
 	}
 
-protected:
 	virtual void handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("RtabmapEvent") == 0)
@@ -219,20 +272,22 @@ protected:
 			OdometryEvent * odomEvent = (OdometryEvent *)event;
 			// Odometry must be processed in the Qt thread
 			if(this->isVisible() &&
-			   _lastOdometryProcessed &&
+			   lastOdometryProcessed_ &&
-			   !_processingStatistics)
+			   !processingStatistics_)
 			{
-				_lastOdometryProcessed = false; // if we receive too many odometry events!
+				lastOdometryProcessed_ = false; // if we receive too many odometry events!
 				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
 			}
 		}
 	}
 
-private:
+protected:
 	CloudViewer * cloudViewer_;
+	CameraThread * camera_;
 	Transform lastOdomPose_;
-	bool _processingStatistics;
+	Transform odometryCorrection_;
-	bool _lastOdometryProcessed;
+	bool processingStatistics_;
+	bool lastOdometryProcessed_;
 };
 
 

examples/RGBDMapping/main.cpp

@@ -65,10 +65,6 @@ int main(int argc, char * argv[])
 		}
 	}
 
-	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
-	QApplication app(argc, argv);
-	MapBuilder mapBuilder;
-
 	// Here is the pipeline that we will use:
 	// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
 
@@ -124,6 +120,11 @@ int main(int argc, char * argv[])
 		exit(1);
 	}
 
+	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
+	// We give it the camera so the GUI can pause/resume the camera
+	QApplication app(argc, argv);
+	MapBuilder mapBuilder(&cameraThread);
+
 	// Create an odometry thread to process camera events, it will send OdometryEvent.
 	OdometryThread odomThread(new OdometryBOW());
 

examples/WifiMapping/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES}
+	${QT_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+ELSE()
+    QT5_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+ENDIF()
+
+ADD_EXECUTABLE(wifi_mapping main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(wifi_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( wifi_mapping 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)

examples/WifiMapping/MapBuilderWifi.h

@@ -0,0 +1,175 @@
+/*
+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.
+*/
+
+#ifndef MAPBUILDERWIFI_H_
+#define MAPBUILDERWIFI_H_
+
+#include "../RGBDMapping/MapBuilder.h"
+
+using namespace rtabmap;
+
+class MapBuilderWifi : public MapBuilder
+{
+	Q_OBJECT
+public:
+	// Camera ownership is not transferred!
+	MapBuilderWifi(CameraThread * camera = 0) :
+		MapBuilder(camera)
+	{}
+
+	virtual ~MapBuilderWifi()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected slots:
+	virtual void processStatistics(const rtabmap::Statistics & stats)
+	{
+		processingStatistics_ = true;
+
+		const std::map<int, Transform> & poses = stats.poses();
+		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
+
+		//============================
+		// Add WIFI symbols
+		//============================
+
+		// Sort stamps by stamps
+		std::map<double, int> nodeStamps; // <stamp, id>
+		for(std::map<int, double>::const_iterator iter = stats.getStamps().begin(); iter!=stats.getStamps().end(); ++iter)
+		{
+			nodeStamps.insert(std::make_pair(iter->second, iter->first));
+		}
+
+		// convert userData to wifi levels
+		std::map<int, std::pair<int, double> > wifiLevels;
+		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stats.getUserDatas().begin(); iter!=stats.getUserDatas().end(); ++iter)
+		{
+			UASSERT(iter->second.size() == sizeof(int)+sizeof(double));
+
+			// format [int level, double stamp]
+			int level;
+			double stamp;
+			memcpy(&level, iter->second.data(), sizeof(int));
+			memcpy(&stamp, iter->second.data()+sizeof(int), sizeof(double));
+
+			wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp)));
+		}
+
+		for(std::map<int, std::pair<int, double> >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter)
+		{
+			// The Wifi value may be taken between two nodes, interpolate its position.
+			double stampWifi = iter->second.second;
+			std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
+			if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
+			{
+				--previousNode;
+			}
+			std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(iter->second.second); // upper bound of the stamp
+
+			if(previousNode != nodeStamps.end() && nextNode != nodeStamps.end() &&
+			   previousNode->second != nextNode->second &&
+			   uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
+			{
+				Transform poseA = poses.at(previousNode->second);
+				Transform poseB = poses.at(nextNode->second);
+				double stampA = previousNode->first;
+				double stampB = nextNode->first;
+				UASSERT(stampWifi>=stampA && stampWifi <=stampB);
+
+				Transform v = poseA.inverse() * poseB;
+				double ratio = (stampWifi-stampA)/(stampB-stampA);
+
+				v.x()*=ratio;
+				v.y()*=ratio;
+				v.z()*=ratio;
+
+				Transform wifiPose = (poseA*v).translation(); // rip off the rotation
+
+				std::string cloudName = uFormat("level%d", iter->first);
+				if(clouds.contains(cloudName))
+				{
+					if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
+					{
+						UERROR("Updating pose cloud %d failed!", iter->first);
+					}
+				}
+				else
+				{
+					// Make a line with points
+					int level = -iter->second.first;
+					level = level < 30 ? 0 : level > 80 ? 50: level - 30; // set between 0 and 50
+					level/=5; // set between 0 and 10
+					level = 10 - level; // make higher level to 10
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+					for(int i=0; i<10; ++i)
+					{
+						// 2 cm between each points
+						// the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
+						pcl::PointXYZRGB pt;
+						pt.z = float(i+1)*0.02f;
+						if(i<level)
+						{
+							// yellow
+							pt.r = 255;
+							pt.g = 255;
+						}
+						else
+						{
+							// gray
+							pt.r = pt.g = pt.b = 100;
+						}
+						cloud->push_back(pt);
+					}
+					pcl::PointXYZRGB anchor(255, 0, 0);
+					cloud->push_back(anchor);
+					//UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size());
+					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, wifiPose, Qt::yellow))
+					{
+						UERROR("Adding cloud %d to viewer failed!", iter->first);
+					}
+					else
+					{
+						cloudViewer_->setCloudPointSize(cloudName, 5);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Bounds not found!");
+			}
+		}
+
+		//============================
+		// Add RGB-D clouds
+		//============================
+		MapBuilder::processStatistics(stats);
+	}
+};
+
+
+#endif /* MAPBUILDERWIFI_H_ */

examples/WifiMapping/WifiThread.h

@@ -0,0 +1,106 @@
+/*
+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.
+*/
+
+#ifndef WIFITHREAD_H_
+#define WIFITHREAD_H_
+
+#include <sys/socket.h>
+#include <linux/wireless.h>
+#include <sys/ioctl.h>
+#include <rtabmap/core/UserDataEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+
+class WifiThread : public UThread, public UEventsSender
+{
+public:
+	WifiThread(const std::string & interfaceName, float rate = 0.5) :
+		interfaceName_(interfaceName),
+		rate_(rate)
+	{}
+	virtual ~WifiThread() {}
+
+private:
+	virtual void mainLoop()
+	{
+		uSleep(1000/rate_);
+		if(!this->isKilled())
+		{
+			// Code inspired from http://blog.ajhodges.com/2011/10/using-ioctl-to-gather-wifi-information.html
+
+			//have to use a socket for ioctl
+			int sockfd;
+			/* Any old socket will do, and a datagram socket is pretty cheap */
+			if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+				UERROR("Could not create simple datagram socket");
+				return;
+			}
+
+			struct iwreq req;
+			struct iw_statistics stats;
+
+			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ);
+
+			//make room for the iw_statistics object
+			req.u.data.pointer = (caddr_t) &stats;
+			req.u.data.length = sizeof(stats);
+			// clear updated flag
+			req.u.data.flags = 1;
+
+			//this will gather the signal strength
+			if(ioctl(sockfd, SIOCGIWSTATS, &req) == -1)
+			{
+				//die with error, invalid interface
+				UERROR("Invalid interface (\"%s\"). Tip: Try with sudo!", interfaceName_.c_str());
+			}
+			else if(((iw_statistics *)req.u.data.pointer)->qual.updated & IW_QUAL_DBM)
+			{
+				//signal is measured in dBm and is valid for us to use
+				int level = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
+				double stamp = UTimer::now();
+
+				// Create user data [level, stamp] with the value (int = 4 bytes) and a timestamp (double = 8 bytes)
+				std::vector<unsigned char> data(sizeof(int) + sizeof(double));
+				memcpy(data.data(), &level, sizeof(int));
+				memcpy(data.data()+sizeof(int), &stamp, sizeof(double));
+				this->post(new UserDataEvent(data));
+				//UWARN("posting level %d dBm", level);
+			}
+			else
+			{
+				UERROR("Could not get signal level.");
+			}
+
+			close(sockfd);
+		}
+	}
+
+private:
+	std::string interfaceName_;
+	float rate_;
+};
+
+#endif /* WIFITHREAD_H_ */

examples/WifiMapping/main.cpp

@@ -0,0 +1,191 @@
+/*
+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/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <QApplication>
+#include <stdio.h>
+
+#include "MapBuilderWifi.h"
+#include "WifiThread.h"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-wifi_mapping interface_name [driver]\n"
+			"  interface_name         Wifi interface name (e.g. \"eth0\")\n"
+			"  driver                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+	exit(1);
+}
+
+using namespace rtabmap;
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	std::string interfaceName;
+	int driver = 0;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		interfaceName = argv[1];
+		if(interfaceName.size() == 0)
+		{
+			UERROR("Interface name invalid!");
+			showUsage();
+		}
+		else
+		{
+			printf("Using Wifi interface \"%s\"\n", interfaceName.c_str());
+		}
+		if(argc > 2)
+		{
+			driver = atoi(argv[2]);
+			if(driver < 0 || driver > 4)
+			{
+				UERROR("driver should be between 0 and 4.");
+				showUsage();
+			}
+		}
+	}
+
+	// Here is the pipeline that we will use:
+	// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
+	// Set transform to camera so z is up, y is left and x going forward
+	CameraRGBD * camera = 0;
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	if(driver == 1)
+	{
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNI2("", 0, opticalRotation);
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect(0, 0, opticalRotation);
+	}
+	else if(driver == 3)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(false, 0, opticalRotation);
+	}
+	else if(driver == 4)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(true, 0, opticalRotation);
+	}
+	else
+	{
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+	}
+
+	CameraThread cameraThread(camera);
+	if(!cameraThread.init())
+	{
+		UERROR("Camera init failed!");
+		//exit(1);
+	}
+
+	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
+	// We give it the camera so the GUI can pause/resume the camera
+	QApplication app(argc, argv);
+	MapBuilderWifi mapBuilderWifi(&cameraThread);
+
+	// Create an odometry thread to process camera events, it will send OdometryEvent.
+	OdometryThread odomThread(new OdometryBOW());
+
+	// Create RTAB-Map to process OdometryEvent
+	Rtabmap * rtabmap = new Rtabmap();
+	ParametersMap param;
+	param.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // disable rehearsal (node merging when not moving)
+	rtabmap->init(param);
+	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
+
+	// Create Wifi monitoring thread
+	WifiThread wifiThread(interfaceName); // 0.5 Hz, should be under RTAB-Map rate (which is 1 Hz by default)
+
+	// Setup handlers
+	odomThread.registerToEventsManager();
+	rtabmapThread.registerToEventsManager();
+	mapBuilderWifi.registerToEventsManager();
+
+	// The RTAB-Map is subscribed by default to CameraEvent, but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.
+	// We can do that by creating a "pipe" between the camera and odometry, then
+	// only the odometry will receive CameraEvent from that camera. RTAB-Map is
+	// also subscribed to OdometryEvent by default, so no need to create a pipe between
+	// odometry and RTAB-Map.
+	UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
+
+	// Let's start the threads
+	rtabmapThread.start();
+	odomThread.start();
+	cameraThread.start();
+	wifiThread.start();
+
+	mapBuilderWifi.show();
+	app.exec(); // main loop
+
+	// remove handlers
+	mapBuilderWifi.unregisterFromEventsManager();
+	rtabmapThread.unregisterFromEventsManager();
+	odomThread.unregisterFromEventsManager();
+
+	// Kill all threads
+	cameraThread.kill();
+	odomThread.join(true);
+	rtabmapThread.join(true);
+	wifiThread.join(true);
+
+	return 0;
+}

guilib/include/rtabmap/gui/CloudViewer.h

@@ -75,44 +75,44 @@ public:
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 			const Transform & pose = Transform::getIdentity(),
-			const QColor & color = Qt::gray);
+			const QColor & color = QColor());
 
 	bool addCloud(
 			const std::string & id,
 			const pcl::PCLPointCloud2Ptr & binaryCloud,
 			const Transform & pose,
 			bool rgb,
-			const QColor & color = Qt::gray);
+			const QColor & color = QColor());
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addCloudMesh(
 		const std::string & id,

guilib/src/CloudViewer.cpp

@@ -347,8 +347,12 @@ bool CloudViewer::addCloud(
 		 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
 		 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
 		{
-			 // white
+			QColor c = Qt::gray;
-			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, color.red(), color.green(), color.blue()));
+			if(color.isValid())
+			{
+				c = color;
+			}
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
 			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
 
 			// x,y,z
@@ -367,6 +371,10 @@ bool CloudViewer::addCloud(
 
 				_visualizer->updateColorHandlerIndex(id, 5);
 			}
+			else if(color.isValid())
+			{
+				_visualizer->updateColorHandlerIndex(id, 1);
+			}
 
 			_addedClouds.insert(id, pose);
 			return true;

guilib/src/DatabaseViewer.cpp

@@ -1037,7 +1037,8 @@ void DatabaseViewer::view3DMap()
 						Transform odomPose;
 						std::string label;
 						double stamp;
-						if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true))
+						std::vector<unsigned char> userData;
+						if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
 						{
 							color = (Qt::GlobalColor)(mapId % 12 + 7 );
 						}
@@ -1391,7 +1392,8 @@ void DatabaseViewer::update(int value,
 				int w;
 				std::string l;
 				double s;
-				memory_->getNodeInfo(id, odomPose, mapId, w, l, s, true);
+				std::vector<unsigned char> d;
+				memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true);
 
 				weight->setNum(data.getWeight());
 				label->setText(data.getLabel().c_str());

guilib/src/MainWindow.cpp

@@ -1393,10 +1393,14 @@ void MainWindow::updateMapCloud(
 
 	// update 3D graphes (show all poses)
 	_ui->widget_cloudViewer->removeAllGraphs();
+	_ui->widget_cloudViewer->removeCloud("graph_nodes");
 	if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
 	{
 		// Find all graphs
 		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
+		graphNodes->resize(_currentPosesMap.size());
+		int oi = 0;
 		for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
 		{
 			int mapId = uValue(_currentMapIds, iter->first, -1);
@@ -1405,7 +1409,9 @@ void MainWindow::updateMapCloud(
 			{
 				kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
 			}
-			kter->second->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+			pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
+			kter->second->push_back(pt);
+			(*graphNodes)[oi++] = pcl::PointXYZ(pt);
 		}
 
 		// add graphs
@@ -1418,6 +1424,10 @@ void MainWindow::updateMapCloud(
 			}
 			_ui->widget_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
+
+		// add nodes
+		_ui->widget_cloudViewer->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+		_ui->widget_cloudViewer->setCloudPointSize("graph_nodes", 5);
 	}
 
 	// Update occupancy grid map in 3D map view and graph view

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.8.7</version>
+  <version>0.8.8</version>
   <description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>