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

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() :
-		_processingStatistics(false),
-		_lastOdometryProcessed(true)
+	//Camera ownership is not transferred!
+	MapBuilder(CameraThread * camera = 0) :
+		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:
-	void processOdometry(const rtabmap::SensorData & data)
+protected slots:
+	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 &&
-			   !_processingStatistics)
+			   lastOdometryProcessed_ &&
+			   !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;
-	bool _lastOdometryProcessed;
+	Transform odometryCorrection_;
+	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;
+}