Version 0.10.1: user_data is now a cv::Mat to avoid a deep copy when SensorData is copied

examples/WifiMapping/MapBuilderWifi.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAPBUILDERWIFI_H_
 
 #include "../RGBDMapping/MapBuilder.h"
+#include "rtabmap/core/UserDataEvent.h"
 
 using namespace rtabmap;
 
@@ -64,6 +65,28 @@ public:
 		this->unregisterFromEventsManager();
 	}
 
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("UserDataEvent") == 0)
+		{
+			UserDataEvent * rtabmapEvent = (UserDataEvent *)event;
+			// convert userData to wifi levels
+			if(!rtabmapEvent->data().empty())
+			{
+				UASSERT(rtabmapEvent->data().type() == CV_64FC1 &&
+						rtabmapEvent->data().cols == 2 &&
+						rtabmapEvent->data().rows == 1);
+
+				// format [int level, double stamp]
+				int level = rtabmapEvent->data().at<double>(0);
+				double stamp = rtabmapEvent->data().at<double>(1);
+				wifiLevels_.insert(std::make_pair(stamp, level));
+			}
+		}
+		MapBuilder::handleEvent(event);
+	}
+
 protected slots:
 	virtual void processStatistics(const rtabmap::Statistics & stats)
 	{
@@ -76,7 +99,6 @@ protected slots:
 		// Add WIFI symbols
 		//============================
 		std::map<double, int> nodeStamps; // <stamp, id>
-		std::map<int, std::pair<int, double> > wifiLevels;
 
 		for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
 			iter!=stats.getSignatures().end();
@@ -84,34 +106,22 @@ protected slots:
 		{
 			// Sort stamps by stamps->id
 			nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
-
-			// convert userData to wifi levels
-			if(iter->second.getUserData().size())
-			{
-				UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double));
-
-				// format [int level, double stamp]
-				int level;
-				double stamp;
-				memcpy(&level, iter->second.getUserData().data(), sizeof(int));
-				memcpy(&stamp, iter->second.getUserData().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)
+		int id = 0;
+		for(std::map<double, int>::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id)
 		{
 			// The Wifi value may be taken between two nodes, interpolate its position.
-			double stampWifi = iter->second.second;
+			double stampWifi = iter->first;
 			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
+			std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
 
-			if(previousNode != nodeStamps.end() && nextNode != nodeStamps.end() &&
+			if(previousNode != nodeStamps.end() &&
+			   nextNode != nodeStamps.end() &&
 			   previousNode->second != nextNode->second &&
 			   uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
 			{
@@ -130,18 +140,18 @@ protected slots:
 
 				Transform wifiPose = (poseA*v).translation(); // rip off the rotation
 
-				std::string cloudName = uFormat("level%d", iter->first);
+				std::string cloudName = uFormat("level%d", id);
 				if(clouds.contains(cloudName))
 				{
 					if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
 					{
-						UERROR("Updating pose cloud %d failed!", iter->first);
+						UERROR("Updating pose cloud %d failed!", id);
 					}
 				}
 				else
 				{
 					// Make a line with points
-					int quality = dBm2Quality(iter->second.first)/10;
+					int quality = dBm2Quality(iter->second)/10;
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					for(int i=0; i<10; ++i)
 					{
@@ -167,7 +177,7 @@ protected slots:
 					//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);
+						UERROR("Adding cloud %d to viewer failed!", id);
 					}
 					else
 					{
@@ -175,10 +185,6 @@ protected slots:
 					}
 				}
 			}
-			else
-			{
-				UWARN("Bounds not found!");
-			}
 		}
 
 		//============================
@@ -186,6 +192,9 @@ protected slots:
 		//============================
 		MapBuilder::processStatistics(stats);
 	}
+
+private:
+	std::map<double, int> wifiLevels_;
 };
 
 

examples/WifiMapping/WifiThread.h

@@ -207,10 +207,10 @@ private:
 			{
 				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(), &dBm, sizeof(int));
-				memcpy(data.data()+sizeof(int), &stamp, sizeof(double));
+				// Create user data [level, stamp] with the value and a timestamp
+				cv::Mat data(1, 2, CV_64FC1);
+				data.at<double>(0) = double(dBm);
+				data.at<double>(1) = stamp;
 				this->post(new UserDataEvent(data));
 				//UWARN("posting level %d dBm", dBm);
 			}

examples/WifiMapping/main.cpp

@@ -56,7 +56,7 @@ int main(int argc, char * argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
-	std::string interfaceName = "eth0";
+	std::string interfaceName = "wlan0";
 	int driver = 0;
 	bool mirroring = false;
 
@@ -155,8 +155,9 @@ int main(int argc, char * argv[])
 
 	if(!camera->init())
 	{
-		UERROR("Camera init failed!");
-		//exit(1);
+		UERROR("Camera init failed! Try another camera driver.");
+		showUsage();
+		exit(1);
 	}
 	CameraThread cameraThread(camera);
 	if(mirroring)