updated some events with user_data/stamp

corelib/include/rtabmap/core/Rtabmap.h

@@ -117,13 +117,17 @@ public:
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			std::map<int, int> & mapIds,
+			std::map<int, double> & stamps,
 			std::map<int, std::string> & labels,
+			std::map<int, std::vector<unsigned char> > & userDatas,
 			bool optimized,
 			bool global) const;
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			std::map<int, int> & mapIds,
+			std::map<int, double> & stamps,
 			std::map<int, std::string> & labels,
+			std::map<int, std::vector<unsigned char> > & userDatas,
 			bool optimized,
 			bool global);
 	void clearPath();

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -149,13 +149,17 @@ public:
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds,
-			const std::map<int, std::string> & labels) :
+			const std::map<int, double> & stamps,
+			const std::map<int, std::string> & labels,
+			const std::map<int, std::vector<unsigned char> > & userDatas) :
 		UEvent(0),
 		_signatures(signatures),
 		_poses(poses),
 		_constraints(constraints),
 		_mapIds(mapIds),
-		_labels(labels)
+		_stamps(stamps),
+		_labels(labels),
+		_userDatas(userDatas)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -164,7 +168,9 @@ public:
 	const std::map<int, Transform> & getPoses() const {return _poses;}
 	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
 	const std::map<int, int> & getMapIds() const {return _mapIds;}
+	const std::map<int, double> & getStamps() const {return _stamps;}
 	const std::map<int, std::string> & getLabels() const {return _labels;}
+	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -173,7 +179,9 @@ private:
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
 	std::map<int, int> _mapIds;
+	std::map<int, double> _stamps;
 	std::map<int, std::string> _labels;
+	std::map<int, std::vector<unsigned char> > _userDatas;
 };
 
 } // namespace rtabmap

corelib/src/Rtabmap.cpp

@@ -1733,10 +1733,7 @@ bool Rtabmap::process(const SensorData & data)
 					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));
-					{
-						userDatas.insert(std::make_pair(iter->first, userData));
-					}
 				}
 				statistics_.setPoses(poses);
 				statistics_.setConstraints(constraints);
@@ -1932,10 +1929,7 @@ bool Rtabmap::process(const SensorData & data)
 				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));
-				{
-					userDatas.insert(std::make_pair(iter->first, userData));
-				}
 			}
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
@@ -2381,7 +2375,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		std::map<int, int> & mapIds,
+		std::map<int, double> & stamps,
 		std::map<int, std::string> & labels,
+		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
 		bool global) const
 {
@@ -2417,7 +2413,9 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			std::vector<unsigned char> userData;
 			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
 			mapIds.insert(std::make_pair(iter->first, mapId));
+			stamps.insert(std::make_pair(iter->first, stamp));
 			labels.insert(std::make_pair(iter->first, label));
+			userDatas.insert(std::make_pair(iter->first, userData));
 		}
 
 
@@ -2456,7 +2454,9 @@ void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		std::map<int, int> & mapIds,
+		std::map<int, double> & stamps,
 		std::map<int, std::string> & labels,
+		std::map<int, std::vector<unsigned char> > & userDatas,
 		bool optimized,
 		bool global)
 {
@@ -2491,7 +2491,9 @@ void Rtabmap::getGraph(
 			std::vector<unsigned char> userData;
 			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
 			mapIds.insert(std::make_pair(iter->first, mapId));
+			stamps.insert(std::make_pair(iter->first, stamp));
 			labels.insert(std::make_pair(iter->first, label));
+			userDatas.insert(std::make_pair(iter->first, userData));
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
@@ -2603,8 +2605,10 @@ bool Rtabmap::computePath(int targetNode, bool global)
 	std::map<int, Transform> nodes;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
-	this->getGraph(nodes, constraints, mapIds, labels, true, global);
+	std::map<int, std::vector<unsigned char> > userDatas;
+	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	if(computePath(targetNode, nodes, constraints))
@@ -2632,8 +2636,10 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	std::map<int, Transform> nodes;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
-	this->getGraph(nodes, constraints, mapIds, labels, true, global);
+	std::map<int, std::vector<unsigned char> > userDatas;
+	this->getGraph(nodes, constraints, mapIds, stamps, labels, userDatas, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

corelib/src/RtabmapThread.cpp

@@ -117,13 +117,17 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 
 	_rtabmap->get3DMap(signatures,
 			poses,
 			constraints,
 			mapIds,
+			stamps,
 			labels,
+			userDatas,
 			optimized,
 			full);
 
@@ -131,7 +135,9 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 			poses,
 			constraints,
 			mapIds,
-			labels));
+			stamps,
+			labels,
+			userDatas));
 }
 
 void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
@@ -140,12 +146,16 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 
 	_rtabmap->getGraph(poses,
 			constraints,
 			mapIds,
+			stamps,
 			labels,
+			userDatas,
 			optimized,
 			full);
 
@@ -153,7 +163,9 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 			poses,
 			constraints,
 			mapIds,
-			labels));
+			stamps,
+			labels,
+			userDatas));
 }
 
 

examples/WifiMapping/MapBuilderWifi.h

@@ -57,27 +57,30 @@ protected slots:
 		//============================
 		// 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(stats.getStamps().size() == stats.getUserDatas().size());
+		std::map<int, double>::const_iterator iterStamps = stats.getStamps().begin();
+		std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = stats.getUserDatas().begin();
+		for(; iterStamps!=stats.getStamps().end() && iterUserDatas!=stats.getUserDatas().end(); ++iterStamps, ++iterUserDatas)
 		{
-			UASSERT(iter->second.size() == sizeof(int)+sizeof(double));
+			// Sort stamps by stamps
+			nodeStamps.insert(std::make_pair(iterStamps->second, iterStamps->first));
 
-			// format [int level, double stamp]
+			// convert userData to wifi levels
-			int level;
+			if(iterUserDatas->second.size())
-			double stamp;
+			{
-			memcpy(&level, iter->second.data(), sizeof(int));
+				UASSERT(iterUserDatas->second.size() == sizeof(int)+sizeof(double));
-			memcpy(&stamp, iter->second.data()+sizeof(int), sizeof(double));
 
-			wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp)));
+				// format [int level, double stamp]
+				int level;
+				double stamp;
+				memcpy(&level, iterUserDatas->second.data(), sizeof(int));
+				memcpy(&stamp, iterUserDatas->second.data()+sizeof(int), sizeof(double));
+
+				wifiLevels.insert(std::make_pair(iterUserDatas->first, std::make_pair(level, stamp)));
+			}
 		}
 
 		for(std::map<int, std::pair<int, double> >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter)