feat(slam): add rtabmap_ros

rtabmap/examples/WifiMapping/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.14)
+
+if(POLICY CMP0020)
+    cmake_policy(SET CMP0020 NEW)
+endif()
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(NOT internal)
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+endif()
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+	SET(moc_srcs MapBuilder.h MapBuilderWifi.h)
+ENDIF()
+
+SET(srcs
+    main.cpp)
+
+set(LIBRARIES "")
+
+IF(APPLE)
+    FIND_LIBRARY(CoreWLAN_LIBRARY CoreWLAN)
+    FIND_LIBRARY(Foundation_LIBRARY Foundation)
+    MARK_AS_ADVANCED(CoreWLAN_LIBRARY Foundation_LIBRARY)
+    SET(LIBRARIES
+	    ${LIBRARIES}
+	    ${CoreWLAN_LIBRARY}
+	    ${Foundation_LIBRARY}
+	)
+	SET(srcs
+	    ${srcs}
+	    WifiOSX.mm
+	)
+ENDIF(APPLE)
+
+ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs})
+TARGET_LINK_LIBRARIES(wifi_mapping rtabmap::gui ${LIBRARIES})
+
+SET_TARGET_PROPERTIES(
+     wifi_mapping
+   PROPERTIES
+	 AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+if(internal)
+  SET_TARGET_PROPERTIES( wifi_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)
+endif(internal)

rtabmap/examples/WifiMapping/MapBuilder.h

@@ -0,0 +1,291 @@
+/*
+Copyright (c) 2010-2016, 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 MAPBUILDER_H_
+#define MAPBUILDER_H_
+
+#include <QVBoxLayout>
+#include <QtCore/QMetaType>
+#include <QAction>
+
+#ifndef Q_MOC_RUN // Mac OS X issue
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#endif
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include <rtabmap/core/SensorCaptureThread.h>
+
+using namespace rtabmap;
+
+// This class receives RtabmapEvent and construct/update a 3D Map
+class MapBuilder : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	//Camera ownership is not transferred!
+	MapBuilder(SensorCaptureThread * camera = 0) :
+		camera_(camera),
+		odometryCorrection_(Transform::getIdentity()),
+		processingStatistics_(false),
+		lastOdometryProcessed_(true)
+	{
+		this->setWindowFlags(Qt::Dialog);
+		this->setWindowTitle(tr("3D Map"));
+		this->setMinimumWidth(800);
+		this->setMinimumHeight(600);
+
+		cloudViewer_ = new CloudViewer(this);
+
+		QVBoxLayout *layout = new QVBoxLayout();
+		layout->addWidget(cloudViewer_);
+		this->setLayout(layout);
+
+		qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
+		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
+	}
+
+	virtual ~MapBuilder()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected Q_SLOTS:
+	virtual void pauseDetection()
+	{
+		UWARN("");
+		if(camera_)
+		{
+			if(camera_->isCapturing())
+			{
+				camera_->join(true);
+			}
+			else
+			{
+				camera_->start();
+			}
+		}
+	}
+
+	virtual void processOdometry(const rtabmap::OdometryEvent & odom)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		Transform pose = odom.pose();
+		if(pose.isNull())
+		{
+			//Odometry lost
+			cloudViewer_->setBackgroundColor(Qt::darkRed);
+
+			pose = lastOdomPose_;
+		}
+		else
+		{
+			cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
+		}
+		if(!pose.isNull())
+		{
+			lastOdomPose_ = pose;
+
+			// 3d cloud
+			if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
+			   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
+			   !odom.data().depthOrRightRaw().empty() &&
+			   (odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+					odom.data(),
+					2,     // decimation
+					4.0f); // max depth
+				if(cloud->size())
+				{
+					if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
+					{
+						UERROR("Adding cloudOdom to viewer failed!");
+					}
+				}
+				else
+				{
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					UWARN("Empty cloudOdom!");
+				}
+			}
+
+			if(!odom.pose().isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
+			}
+		}
+		cloudViewer_->update();
+		cloudViewer_->refreshView();
+
+		lastOdometryProcessed_ = true;
+	}
+
+
+	virtual void processStatistics(const rtabmap::Statistics & stats)
+	{
+		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.lower_bound(1); iter!=poses.end(); ++iter)
+		{
+			if(!iter->second.isNull())
+			{
+				std::string cloudName = uFormat("cloud%d", iter->first);
+
+				// 3d point cloud
+				if(clouds.contains(cloudName))
+				{
+					// Update only if the pose has changed
+					Transform tCloud;
+					cloudViewer_->getPose(cloudName, tCloud);
+					if(tCloud.isNull() || iter->second != tCloud)
+					{
+						if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
+						{
+							UERROR("Updating pose cloud %d failed!", iter->first);
+						}
+					}
+					cloudViewer_->setCloudVisibility(cloudName, true);
+				}
+				else if(iter->first == stats.getLastSignatureData().id())
+				{
+					Signature s = stats.getLastSignatureData();
+					s.sensorData().uncompressData(); // make sure data is uncompressed
+					// Add the new cloud
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+						    4,     // decimation
+						    4.0f); // max depth
+					if(cloud->size())
+					{
+						if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
+						{
+							UERROR("Adding cloud %d to viewer failed!", iter->first);
+						}
+					}
+					else
+					{
+						UWARN("Empty cloud %d!", iter->first);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
+		}
+
+		//============================
+		// 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.lower_bound(1); 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_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+			cloudViewer_->setCloudPointSize("graph_nodes", 5);
+		}
+
+		odometryCorrection_ = stats.mapCorrection();
+
+		cloudViewer_->update();
+		cloudViewer_->refreshView();
+
+		processingStatistics_ = false;
+	}
+
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("RtabmapEvent") == 0)
+		{
+			RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
+			const Statistics & stats = rtabmapEvent->getStats();
+			// Statistics must be processed in the Qt thread
+			if(this->isVisible())
+			{
+				QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			}
+		}
+		else if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			OdometryEvent * odomEvent = (OdometryEvent *)event;
+			// Odometry must be processed in the Qt thread
+			if(this->isVisible() &&
+			   lastOdometryProcessed_ &&
+			   !processingStatistics_)
+			{
+				lastOdometryProcessed_ = false; // if we receive too many odometry events!
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
+			}
+		}
+		return false;
+	}
+
+protected:
+	CloudViewer * cloudViewer_;
+	SensorCaptureThread * camera_;
+	Transform lastOdomPose_;
+	Transform odometryCorrection_;
+	bool processingStatistics_;
+	bool lastOdometryProcessed_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

rtabmap/examples/WifiMapping/MapBuilderWifi.h

@@ -0,0 +1,198 @@
+/*
+Copyright (c) 2010-2016, 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 "MapBuilder.h"
+#include "rtabmap/core/UserDataEvent.h"
+
+using namespace rtabmap;
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int dBm2Quality(int dBm)
+{
+	// dBm to Quality:
+    if(dBm <= -100)
+        return 0;
+    else if(dBm >= -50)
+    	return 100;
+    else
+    	return 2 * (dBm + 100);
+}
+
+class MapBuilderWifi : public MapBuilder
+{
+	Q_OBJECT
+public:
+	// Camera ownership is not transferred!
+	MapBuilderWifi(SensorCaptureThread * camera = 0) :
+		MapBuilder(camera)
+	{}
+
+	virtual ~MapBuilderWifi()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected Q_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->id
+		nodeStamps_.insert(std::make_pair(stats.getLastSignatureData().getStamp(), stats.getLastSignatureData().id()));
+
+		if(!stats.getLastSignatureData().sensorData().userDataRaw().empty())
+		{
+			UASSERT(stats.getLastSignatureData().sensorData().userDataRaw().type() == CV_64FC1 &&
+					stats.getLastSignatureData().sensorData().userDataRaw().cols == 2 &&
+					stats.getLastSignatureData().sensorData().userDataRaw().rows == 1);
+
+			// format [int level, double stamp]
+			int level = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(0);
+			double stamp = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(1);
+			wifiLevels_.insert(std::make_pair(stamp, level));
+		}
+
+		// for the logic below, we should keep only stamps for
+		// nodes still in the graph (in case nodes are ignored when not moving)
+		std::map<double, int> nodeStamps;
+		for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
+		{
+			std::map<int, Transform>::const_iterator jter = poses.find(iter->second);
+			if(jter != poses.end())
+			{
+				nodeStamps.insert(*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->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(stampWifi); // 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", id);
+				if(clouds.contains(cloudName))
+				{
+					if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
+					{
+						UERROR("Updating pose cloud %d failed!", id);
+					}
+				}
+				else
+				{
+					// Make a line with points
+					int quality = dBm2Quality(iter->second)/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<quality)
+						{
+							// yellow
+							pt.r = 255;
+							pt.g = 255;
+						}
+						else
+						{
+							// gray
+							pt.r = pt.g = pt.b = 100;
+						}
+						cloud->push_back(pt);
+					}
+					pcl::PointXYZRGB anchor;
+					anchor.r = 255;
+					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_->addCloud(cloudName, cloud, wifiPose, Qt::yellow))
+					{
+						UERROR("Adding cloud %d to viewer failed!", id);
+					}
+					else
+					{
+						cloudViewer_->setCloudPointSize(cloudName, 5);
+					}
+				}
+			}
+		}
+
+		//============================
+		// Add RGB-D clouds
+		//============================
+		MapBuilder::processStatistics(stats);
+	}
+
+private:
+	std::map<double, int> wifiLevels_;
+	std::map<double, int> nodeStamps_; // <stamp, id>
+};
+
+
+#endif /* MAPBUILDERWIFI_H_ */

rtabmap/examples/WifiMapping/WifiOSX.h

@@ -0,0 +1,45 @@
+/*
+Copyright (c) 2010-2016, 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 WIFIOSX_H_
+#define WIFIOSX_H_
+
+#include <string>
+#include <vector>
+
+struct AccessPoint
+{
+    std::string ssid;
+    std::string bssid;
+    int rssi;
+};
+
+int getRssi(const std::string& interfaceName);
+std::vector<AccessPoint> scanAir(const std::string& interfaceName);
+
+
+#endif /* WIFIOSX_H_ */

rtabmap/examples/WifiMapping/WifiOSX.mm

@@ -0,0 +1,34 @@
+#import <CoreWLAN/CoreWLAN.h>
+#include "WifiOSX.h"
+
+int getRssi(const std::string& interfaceName)
+{
+    NSString* ifName = [NSString stringWithUTF8String:interfaceName.c_str()];
+    CWInterface* interface = [CWInterface interfaceWithName:ifName];
+    return interface.rssiValue;
+}
+
+std::vector<AccessPoint> scanAir(const std::string& interfaceName)
+{
+    NSString* ifName = [NSString stringWithUTF8String:interfaceName.c_str()];
+    CWInterface* interface = [CWInterface interfaceWithName:ifName];
+    
+    NSError* error = nil;
+    NSArray* scanResult = [[interface scanForNetworksWithSSID:nil error:&error] allObjects];
+    if (error)
+    {
+        NSLog(@"%@ (%ld)", [error localizedDescription], [error code]);
+    }
+    
+    std::vector<AccessPoint> result;
+    for (CWNetwork* network in scanResult)
+    {
+        AccessPoint ap;
+        ap.ssid  = std::string([[network ssid] UTF8String]);
+        ap.bssid = std::string([[network bssid] UTF8String]);
+        ap.rssi = [network rssiValue];
+        result.push_back(ap);
+    }
+    
+    return result;
+}

rtabmap/examples/WifiMapping/WifiThread.h

@@ -0,0 +1,225 @@
+/*
+Copyright (c) 2010-2016, 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_
+
+#ifdef _WIN32
+	#ifndef UNICODE
+	#define UNICODE
+	#endif
+
+	#include <windows.h>
+	#include <wlanapi.h>
+	#include <Windot11.h>           // for DOT11_SSID struct
+	#include <objbase.h>
+	#include <wtypes.h>
+
+	#include <stdio.h>
+	#include <stdlib.h>
+
+	// Need to link with Wlanapi.lib and Ole32.lib
+	#pragma comment(lib, "wlanapi.lib")
+	#pragma comment(lib, "ole32.lib")
+#elif __APPLE__
+#include "WifiOSX.h"
+#else
+	#include <sys/socket.h>
+	#include <linux/wireless.h>
+	#include <sys/ioctl.h>
+#endif
+#include <rtabmap/core/UserDataEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int quality2dBm(int quality)
+{
+	// Quality to dBm:
+	if(quality <= 0)
+		return -100;
+	else if(quality >= 100)
+		return -50;
+	else
+		return (quality / 2) - 100;
+}
+
+
+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())
+		{
+			int dBm = 0;
+#ifdef _WIN32
+			//From https://msdn.microsoft.com/en-us/library/windows/desktop/ms706765(v=vs.85).aspx
+			// Declare and initialize variables.
+			HANDLE hClient = NULL;
+			DWORD dwMaxClient = 2;      //
+			DWORD dwCurVersion = 0;
+			DWORD dwResult = 0;
+
+			// variables used for WlanEnumInterfaces
+			PWLAN_INTERFACE_INFO_LIST pIfList = NULL;
+			PWLAN_INTERFACE_INFO pIfInfo = NULL;
+
+			// variables used for WlanQueryInterfaces for opcode = wlan_intf_opcode_current_connection
+			PWLAN_CONNECTION_ATTRIBUTES pConnectInfo = NULL;
+			DWORD connectInfoSize = sizeof(WLAN_CONNECTION_ATTRIBUTES);
+			WLAN_OPCODE_VALUE_TYPE opCode = wlan_opcode_value_type_invalid;
+
+			dwResult = WlanOpenHandle(dwMaxClient, NULL, &dwCurVersion, &hClient);
+			if (dwResult != ERROR_SUCCESS)
+			{
+				UERROR("WlanOpenHandle failed with error: %u\n", dwResult);
+			}
+			else
+			{
+				dwResult = WlanEnumInterfaces(hClient, NULL, &pIfList);
+				if (dwResult != ERROR_SUCCESS)
+				{
+					UERROR("WlanEnumInterfaces failed with error: %u\n", dwResult);
+				}
+				else
+				{
+					// take the first interface found
+					int i = 0;
+					pIfInfo = (WLAN_INTERFACE_INFO *) & pIfList->InterfaceInfo[i];
+					if(pIfInfo->isState == wlan_interface_state_connected)
+					{
+						dwResult = WlanQueryInterface(hClient,
+													  &pIfInfo->InterfaceGuid,
+													  wlan_intf_opcode_current_connection,
+													  NULL,
+													  &connectInfoSize,
+													  (PVOID *) &pConnectInfo,
+													  &opCode);
+
+						if (dwResult != ERROR_SUCCESS)
+						{
+							UERROR("WlanQueryInterface failed with error: %u\n", dwResult);
+						}
+						else
+						{
+							int quality = pConnectInfo->wlanAssociationAttributes.wlanSignalQuality;
+							dBm = quality2dBm(quality);
+						}
+					}
+					else
+					{
+						UERROR("Interface not connected!");
+					}
+				}
+			}
+			if (pConnectInfo != NULL)
+			{
+				WlanFreeMemory(pConnectInfo);
+				pConnectInfo = NULL;
+			}
+
+			if (pIfList != NULL)
+			{
+				WlanFreeMemory(pIfList);
+				pIfList = NULL;
+			}
+#elif __APPLE__
+			dBm = getRssi(interfaceName_);
+#else
+			// 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
+				dBm = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
+			}
+			else
+			{
+				UERROR("Could not get signal level.");
+			}
+
+			close(sockfd);
+#endif
+			if(dBm != 0)
+			{
+				double stamp = UTimer::now();
+
+				// 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);
+			}
+		}
+	}
+
+private:
+	std::string interfaceName_;
+	float rate_;
+};
+
+#endif /* WIFITHREAD_H_ */

rtabmap/examples/WifiMapping/main.cpp

@@ -0,0 +1,285 @@
+/*
+Copyright (c) 2010-2016, 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/Odometry.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <QApplication>
+#include <stdio.h>
+
+#ifdef RTABMAP_PYTHON
+#include "rtabmap/core/PythonInterface.h"
+#endif
+
+#include "MapBuilderWifi.h"
+
+#include "WifiThread.h"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-wifi_mapping [options]\n"
+			"Options:\n"
+			"  -i \"name\"            Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
+			"  -m                     Enable mirroring of the camera image.\n"
+			"  -d #                   Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=ZED SDK, 7=RealSense, 8=RealSense2 9=Kinect for Azure SDK 10=MYNT EYE S\n\n");
+	exit(1);
+}
+
+using namespace rtabmap;
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+#ifdef RTABMAP_PYTHON
+	PythonInterface python; // Make sure we initialize python in main thread
+#endif
+
+	std::string interfaceName = "wlan0";
+	int driver = 0;
+	bool mirroring = false;
+
+	// parse options
+	for(int i = 1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-i") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				interfaceName = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-m") == 0)
+		{
+			mirroring = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-d") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				driver = atoi(argv[i]);
+				if(driver < 0 || driver > 8)
+				{
+					UERROR("driver should be between 0 and 8.");
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		UERROR("Option \"%s\" not recognized!", argv[i]);
+		showUsage();
+	}
+
+	// Here is the pipeline that we will use:
+	// CameraOpenni -> "" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// Create the OpenNI camera, it will send a  at the rate specified.
+	// Set transform to camera so z is up, y is left and x going forward
+	Camera * 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("", CameraOpenNI2::kTypeColorDepth, 0, opticalRotation);
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect(0, CameraFreenect::kTypeColorDepth, 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 if (driver == 5)
+	{
+		if (!CameraFreenect2::available())
+		{
+			UERROR("Not built with Freenect2 support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD, 0, opticalRotation);
+	}
+	else if (driver == 6)
+	{
+		if (!CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED SDK support...");
+			exit(-1);
+		}
+		camera = new CameraStereoZed(0, -1, 1, 1, 100, false, 0, opticalRotation);
+	}
+	else if (driver == 7)
+	{
+		if (!CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation);
+	}
+	else if (driver == 8)
+	{
+		if (!CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense2("", 0, opticalRotation);
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(1);
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye();
+	}
+	else
+	{
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+	}
+
+
+	if(!camera->init())
+	{
+		UERROR("Camera init failed! Try another camera driver.");
+		showUsage();
+		exit(1);
+	}
+	SensorCaptureThread cameraThread(camera);
+	if(mirroring)
+	{
+		cameraThread.setMirroringEnabled(true);
+	}
+
+	// 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(Odometry::create());
+
+	// 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)
+	param.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0")); // disable node ignored when not moving
+	param.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0")); // disable node ignored 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 , but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the .
+	// We can do that by creating a "pipe" between the camera and odometry, then
+	// only the odometry will receive  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, "");
+
+	// 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;
+}