feat(slam): add rtabmap_ros

rtabmap/examples/BOWMapping/CMakeLists.txt

@@ -0,0 +1,24 @@
+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()
+
+ADD_EXECUTABLE(bow_mapping main.cpp)
+TARGET_LINK_LIBRARIES(bow_mapping rtabmap::rtabmap)
+
+if(internal)
+  SET_TARGET_PROPERTIES( bow_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)
+endif(internal)

rtabmap/examples/BOWMapping/main.cpp

@@ -0,0 +1,174 @@
+/*
+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/Rtabmap.h"
+#include "rtabmap/core/CameraRGB.h"
+#include <opencv2/core/core.hpp>
+#include "rtabmap/utilite/UFile.h"
+#include <stdio.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-bow_mapping [options] \"path\"\n"
+			"  path       Path to a directory of images\n "
+			"  Options:"
+			"     -l   localization mode: use already built RTAB-Map database to localize\n ");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	//ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+
+	std::string path;
+	bool localizationMode = false;
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "-l") == 0)
+		{
+			localizationMode = true;
+		}
+		else
+		{
+			printf("Unrecognized option \"%s\"\n", argv[i]);
+			showUsage();
+		}
+	}
+
+	path = argv[argc-1];
+
+	// rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
+	rtabmap::CameraImages camera(path);
+	if(!camera.init())
+	{
+		printf("Camera init failed, using path \"%s\"\n", path.c_str());
+		exit(1);
+	}
+
+	// Create RTAB-Map
+	rtabmap::Rtabmap rtabmap;
+
+	// Set the time threshold
+	rtabmap.setTimeThreshold(700.0f); // Time threshold : 700 ms, 0 ms means no limit
+
+	// To set other parameters, the Parameters interface must be used (Parameters.h).
+	// Example here to change the loop closure threshold (default 0.15).
+	// Lower the threshold, more loop closures are detected but there is more chance of false positives.
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
+
+	// The time threshold set above is also a parameter, one could have set it the same way:
+	//   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), "700"));
+	// Or SURF hessian treshold:
+	//   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));
+
+	// Appearance-based only, disable RGB-D mode
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false"));
+
+	std::string databasePath = rtabmap::Parameters::createDefaultWorkingDirectory()+"/"+rtabmap::Parameters::getDefaultDatabaseName();
+	if(localizationMode)
+	{
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
+	}
+	else
+	{
+		// delete previous database if there's one...
+		UFile::erase(databasePath);
+	}
+
+	// Initialize rtabmap: delete/create database...
+	rtabmap.init(parameters, databasePath);
+
+	// Process each image of the directory...
+	printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
+
+	int countLoopDetected=0;
+	int i=0;
+	rtabmap::SensorData data = camera.takeImage();
+	int nextIndex = rtabmap.getLastLocationId()+1;
+	while(!data.imageRaw().empty())
+	{
+		// Process image : Main loop of RTAB-Map
+		rtabmap.process(data.imageRaw(), nextIndex);
+
+		// Check if a loop closure is detected and print some info
+		if(rtabmap.getLoopClosureId())
+		{
+			++countLoopDetected;
+		}
+		++i;
+		if(rtabmap.getLoopClosureId())
+		{
+			printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
+					i,
+					rtabmap.getLastProcessTime(),
+					(int)rtabmap.getSTM().size(), // short-term memory
+					(int)rtabmap.getWM().size(), // working memory
+					rtabmap.getLoopClosureId(),
+					rtabmap.getLoopClosureValue(),
+					nextIndex,
+					rtabmap.getLoopClosureId());
+		}
+		else
+		{
+			printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
+					i,
+					rtabmap.getLastProcessTime(),
+					(int)rtabmap.getSTM().size(), // short-term memory
+					(int)rtabmap.getWM().size(), // working memory
+					rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
+					rtabmap.getLoopClosureValue());
+		}
+
+		++nextIndex;
+
+		//Get next image
+		data = camera.takeImage();
+	}
+
+	printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
+
+	// Generate a graph for visualization with Graphiz
+	rtabmap.generateDOTGraph("Graph.dot");
+	printf("Generated graph \"Graph.dot\", viewable with Graphiz using \"neato -Tpdf Graph.dot -o out.pdf\"\n");
+
+	// Cleanup... save database and logs
+	printf("Saving Long-Term Memory to \"rtabmap.db\"...\n");
+	rtabmap.close();
+
+	return 0;
+}

rtabmap/examples/CMakeLists.txt

@@ -0,0 +1,15 @@
+
+ADD_SUBDIRECTORY( BOWMapping )
+
+IF(TARGET rtabmap_gui)
+   ADD_SUBDIRECTORY( RGBDMapping )
+   ADD_SUBDIRECTORY( WifiMapping )
+   ADD_SUBDIRECTORY( NoEventsExample )
+   IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
+      ADD_SUBDIRECTORY( LidarMapping )
+   ENDIF()
+ELSE()
+   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
+ENDIF()
+
+

rtabmap/examples/LidarMapping/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.14)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(NOT internal)
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui)
+
+endif()
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+    SET(moc_srcs MapBuilder.h)
+ENDIF()
+
+ADD_EXECUTABLE(lidar_mapping main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(lidar_mapping rtabmap::gui)
+
+SET_TARGET_PROPERTIES(
+     lidar_mapping
+   PROPERTIES
+     AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+if(internal)
+  SET_TARGET_PROPERTIES( lidar_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_mapping)
+endif(internal)

rtabmap/examples/LidarMapping/MapBuilder.h

@@ -0,0 +1,340 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+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 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"
+#include "rtabmap/core/OccupancyGrid.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) :
+		sensorCaptureThread_(camera),
+		odometryCorrection_(Transform::getIdentity()),
+		processingStatistics_(false),
+		lastOdometryProcessed_(true),
+		visibility_(0)
+	{
+		this->setWindowFlags(Qt::Dialog);
+		this->setWindowTitle(tr("3D Map"));
+		this->setMinimumWidth(800);
+		this->setMinimumHeight(600);
+
+		cloudViewer_ = new CloudViewer(this);
+		cloudViewer_->setCameraTargetLocked(true);
+
+		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()));
+
+		QAction * visibility = new QAction(this);
+		this->addAction(visibility);
+		visibility->setShortcut(Qt::Key_Tab);
+		connect(visibility, SIGNAL(triggered()), this, SLOT(rotateVisibility()));
+	}
+
+	virtual ~MapBuilder()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected Q_SLOTS:
+	virtual void pauseDetection()
+	{
+		UWARN("");
+		if(sensorCaptureThread_)
+		{
+			if(sensorCaptureThread_->isCapturing())
+			{
+				sensorCaptureThread_->join(true);
+			}
+			else
+			{
+				sensorCaptureThread_->start();
+			}
+		}
+	}
+
+	virtual void rotateVisibility()
+	{
+		visibility_ = (visibility_+1) % 3;
+		if(visibility_ == 0)
+		{
+			UWARN("Show both Odom and Map");
+		}
+		else if(visibility_ == 1)
+		{
+			UWARN("Show only Map");
+		}
+		else if(visibility_ == 2)
+		{
+			UWARN("Show only Odom");
+		}
+	}
+
+	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().laserScanRaw().empty())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), odom.data().laserScanRaw().localTransform());
+				if(cloud->size() && (visibility_ == 0 || visibility_ == 2))
+				{
+					if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose, Qt::magenta))
+					{
+						UERROR("Adding cloudOdom to viewer failed!");
+					}
+				}
+				else
+				{
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					if(cloud->empty())
+						UWARN("Empty cloudOdom!");
+				}
+			}
+
+			if(!odom.info().localScanMap.empty())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform());
+				if(cloud->size() && (visibility_ == 0 || visibility_ == 2))
+				{
+					if(!cloudViewer_->addCloud("cloudOdomLocalMap", cloud, odometryCorrection_, Qt::blue))
+					{
+						UERROR("Adding cloudOdomLocalMap to viewer failed!");
+					}
+				}
+				else
+				{
+					cloudViewer_->setCloudVisibility("cloudOdomLocalMap", false);
+					if(cloud->empty())
+						UWARN("Empty cloudOdomLocalMap!");
+				}
+			}
+
+			if(!odom.pose().isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
+			}
+		}
+		cloudViewer_->update();
+
+		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);
+						}
+					}
+				}
+				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::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(
+							s.sensorData().laserScanRaw(),
+						    s.sensorData().laserScanRaw().localTransform());
+					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);
+					}
+				}
+
+				cloudViewer_->setCloudVisibility(cloudName, visibility_ == 0 || visibility_ == 1);
+			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
+		}
+
+		// cleanup
+		for(QMap<std::string, Transform>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
+		{
+			if(uStrContains(iter.key(), "cloud_"))
+			{
+				int id = uStr2Int(uSplitNumChar(iter.key()).back());
+				if(poses.find(id) == poses.end())
+				{
+					cloudViewer_->removeCloud(iter.key());
+				}
+			}
+		}
+
+		//============================
+		// 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();
+
+		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 * sensorCaptureThread_;
+	Transform lastOdomPose_;
+	Transform odometryCorrection_;
+	bool processingStatistics_;
+	bool lastOdometryProcessed_;
+	int visibility_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

rtabmap/examples/LidarMapping/main.cpp

@@ -0,0 +1,244 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+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 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.
+*/
+
+// Should be first on windows to avoid "WinSock.h has already been included" error
+#include "rtabmap/core/lidar/LidarVLP16.h"
+
+#include <rtabmap/core/Odometry.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include <QApplication>
+#include <stdio.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/filters/filter.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+
+#include "MapBuilder.h"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-lidar_mapping IP PORT\n"
+			"rtabmap-lidar_mapping PCAP_FILEPATH\n"
+			"\n"
+			"Example:"
+			"  rtabmap-lidar_mapping 192.168.1.201 2368\n\n");
+	exit(1);
+}
+
+using namespace rtabmap;
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	std::string filepath;
+	std::string ip;
+	int port = 2368;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else if(argc == 2)
+	{
+		filepath = argv[1];
+	}
+	else
+	{
+		ip = argv[1];
+		port = uStr2Int(argv[2]);
+	}
+
+	// Here is the pipeline that we will use:
+	// LidarVLP16 -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// Create the Lidar sensor, it will send a SensorEvent
+	LidarVLP16 * lidar;
+	if(!ip.empty())
+	{
+		printf("Using ip=%s port=%d\n", ip.c_str(), port);
+#if BOOST_VERSION >= 108700  // Version 1.87.0
+		lidar = new LidarVLP16(boost::asio::ip::make_address(ip), port);
+#else
+		lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+#endif
+	}
+	else
+	{
+		filepath = uReplaceChar(filepath, '~', UDirectory::homeDir());
+		printf("Using file=%s\n", filepath.c_str());
+		lidar = new LidarVLP16(filepath);
+	}
+	lidar->setOrganized(true); //faster deskewing
+
+	if(!lidar->init())
+	{
+		UERROR("Lidar init failed!");
+		delete lidar;
+		return -1;
+	}
+
+	SensorCaptureThread lidarThread(lidar);
+
+	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
+	// We give it the lidar so the GUI can pause/resume the lidar
+	QApplication app(argc, argv);
+	MapBuilder mapBuilder(&lidarThread);
+
+	ParametersMap params;
+
+	float resolution = 0.05;
+
+	// ICP parameters
+	params.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
+	params.insert(ParametersPair(Parameters::kIcpFiltersEnabled(), "2"));
+	params.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true"));
+	params.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "20"));
+	params.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), "0"));
+	params.insert(ParametersPair(Parameters::kIcpIterations(), "10"));
+	params.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(resolution)));
+	params.insert(ParametersPair(Parameters::kIcpEpsilon(), "0.001"));
+	params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(resolution*10.0f)));
+	params.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "2"));
+	params.insert(ParametersPair(Parameters::kIcpStrategy(), "1"));
+	params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), "0.7"));
+	params.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.01"));
+	// Uncomment if lidar never pitch/roll (on a car or wheeled robot), for hand-held mapping, keep it commented
+	//params.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), "0.8"));
+
+	// Odom parameters
+	params.insert(ParametersPair(Parameters::kOdomStrategy(), "0")); // F2M
+	params.insert(ParametersPair(Parameters::kOdomScanKeyFrameThr(), "0.6"));
+	params.insert(ParametersPair(Parameters::kOdomF2MScanSubtractRadius(), uNumber2Str(resolution)));
+	params.insert(ParametersPair(Parameters::kOdomF2MScanMaxSize(), "15000"));
+	params.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.3"));
+	params.insert(ParametersPair(Parameters::kOdomDeskewing(), "true"));
+
+	// Create an odometry thread to process lidar events, it will send OdometryEvent.
+	OdometryThread odomThread(Odometry::create(params));
+
+	// Rtabmap params
+	params.insert(ParametersPair(Parameters::kRGBDProximityBySpace(), "true"));
+	params.insert(ParametersPair(Parameters::kRGBDProximityMaxGraphDepth(), "0"));
+	params.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1"));
+	params.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0.05"));
+	params.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0.05"));
+	params.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
+	params.insert(ParametersPair(Parameters::kMemSTMSize(), "30"));
+	uInsert(params, ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.2")); // overwritten
+
+
+	// Create RTAB-Map to process OdometryEvent
+	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->init(params);
+	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
+
+	// Setup handlers
+	odomThread.registerToEventsManager();
+	rtabmapThread.registerToEventsManager();
+	mapBuilder.registerToEventsManager();
+
+	// The RTAB-Map is subscribed by default to SensorEvent, but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent.
+	// We can do that by creating a "pipe" between the lidar and odometry, then
+	// only the odometry will receive SensorEvent from that lidar. RTAB-Map is
+	// also subscribed to OdometryEvent by default, so no need to create a pipe between
+	// odometry and RTAB-Map.
+	UEventsManager::createPipe(&lidarThread, &odomThread, "SensorEvent");
+
+	// Let's start the threads
+	rtabmapThread.start();
+	odomThread.start();
+	lidarThread.start();
+
+	printf("Press Tab key to switch between map and odom views (or both).\n");
+	printf("Press Space key to pause.\n");
+
+	mapBuilder.show();
+	app.exec(); // main loop
+
+	// remove handlers
+	mapBuilder.unregisterFromEventsManager();
+	rtabmapThread.unregisterFromEventsManager();
+	odomThread.unregisterFromEventsManager();
+
+	// Kill all threads
+	lidarThread.kill();
+	odomThread.join(true);
+	rtabmapThread.join(true);
+
+	// Save 3D map
+	printf("Saving rtabmap_cloud.pcd...\n");
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap->getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr tmp = util3d::laserScanToPointCloudI(node.sensorData().laserScanRaw(), node.sensorData().laserScanRaw().localTransform());
+		*cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose
+	}
+	if(cloud->size())
+	{
+		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size());
+		cloud = util3d::voxelize(cloud, 0.01f);
+
+		printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size());
+		pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud);
+		//pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+	}
+	else
+	{
+		printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n");
+	}
+
+	// Save trajectory
+	printf("Saving rtabmap_trajectory.txt ...\n");
+	if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links))
+	{
+		printf("Saving rtabmap_trajectory.txt... done!\n");
+	}
+	else
+	{
+		printf("Saving rtabmap_trajectory.txt... failed!\n");
+	}
+
+	rtabmap->close(false);
+
+	return 0;
+}

rtabmap/examples/NoEventsExample/CMakeLists.txt

@@ -0,0 +1,41 @@
+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 COMPONENTS gui)
+endif()
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+    SET(moc_srcs MapBuilder.h)
+ENDIF()
+
+ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(noEventsExample rtabmap::gui)
+
+SET_TARGET_PROPERTIES(
+     noEventsExample
+   PROPERTIES
+	 AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+if(internal)
+	SET_TARGET_PROPERTIES( noEventsExample 
+	  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample)
+endif(internal)
+
+
+
+

rtabmap/examples/NoEventsExample/MapBuilder.h

@@ -0,0 +1,241 @@
+/*
+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/OdometryInfo.h"
+#include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Signature.h"
+#endif
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+
+using namespace rtabmap;
+
+// This class receives RtabmapEvent and construct/update a 3D Map
+class MapBuilder : public QWidget
+{
+	Q_OBJECT
+public:
+	//Camera ownership is not transferred!
+	MapBuilder() :
+		odometryCorrection_(Transform::getIdentity()),
+		paused_(false)
+	{
+		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);
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
+	}
+
+	virtual ~MapBuilder()
+	{
+	}
+
+	bool isPaused() const {return paused_;}
+
+	void processOdometry(
+			const SensorData & data,
+			Transform pose,
+			const rtabmap::OdometryInfo & odom)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		if(pose.isNull())
+		{
+			//Odometry lost
+			cloudViewer_->setBackgroundColor(Qt::darkRed);
+
+			pose = lastOdomPose_;
+		}
+		else
+		{
+			cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
+		}
+		if(!pose.isNull())
+		{
+			lastOdomPose_ = pose;
+
+			// 3d cloud
+			if(data.depthOrRightRaw().cols == data.imageRaw().cols &&
+			   data.depthOrRightRaw().rows == data.imageRaw().rows &&
+			   !data.depthOrRightRaw().empty() &&
+			   (data.stereoCameraModels().size() || data.cameraModels().size()))
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+					data,
+					4,     // decimation
+					0.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(!pose.isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*pose);
+			}
+		}
+		cloudViewer_->update();
+	}
+
+
+	void processStatistics(const rtabmap::Statistics & stats)
+	{
+
+		//============================
+		// 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();
+	}
+
+protected Q_SLOTS:
+	void pauseDetection()
+	{
+		paused_ = !paused_;
+	}
+
+protected:
+	CloudViewer * cloudViewer_;
+	Transform lastOdomPose_;
+	Transform odometryCorrection_;
+	bool paused_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

rtabmap/examples/NoEventsExample/main.cpp

@@ -0,0 +1,163 @@
+/*
+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/Rtabmap.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/utilite/UThread.h>
+#include "MapBuilder.h"
+#include <pcl/visualization/cloud_viewer.h>
+#include <rtabmap/core/Odometry.h>
+#include <QApplication>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-noEventsExample camera_rate odom_update map_update calibration_dir calibration_name path_left_images path_right_images\n"
+			"Description:\n"
+			"    camera_rate          Rate (Hz) of the camera.\n"
+			"    odom_update          Do odometry update each X camera frames.\n"
+			"    map_update           Do map update each X odometry frames.\n"
+			"\n"
+			"Example:\n"
+			"     (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n"
+			"     $ rtabmap-noEventsExample 20 2 10 stereo_20Hz stereo_20Hz stereo_20Hz/left stereo_20Hz/right\n"
+			"       Camera rate = 20 Hz\n"
+			"       Odometry update rate = 10 Hz\n"
+			"       Map update rate = 1 Hz\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 8)
+	{
+		showUsage();
+	}
+
+	int argIndex = 1;
+	int cameraRate = atoi(argv[argIndex++]);
+	if(cameraRate <= 0)
+	{
+		printf("camera_rate should be > 0\n");
+		showUsage();
+	}
+	int odomUpdate = atoi(argv[argIndex++]);
+	if(odomUpdate <= 0)
+	{
+		printf("odom_update should be > 0\n");
+		showUsage();
+	}
+	int mapUpdate = atoi(argv[argIndex++]);
+	if(mapUpdate <= 0)
+	{
+		printf("map_update should be > 0\n");
+		showUsage();
+	}
+
+	printf("Camera rate = %d Hz\n", cameraRate);
+	printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
+	printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
+
+	std::string calibrationDir = argv[argIndex++];
+	std::string calibrationName = argv[argIndex++];
+	std::string pathLeftImages = argv[argIndex++];
+	std::string pathRightImages = argv[argIndex++];
+
+	CameraStereoImages camera(
+			pathLeftImages,
+			pathRightImages,
+			false, // assume that images are already rectified
+			(float)cameraRate);
+
+	if(camera.init(calibrationDir, calibrationName))
+	{
+		Odometry * odom = Odometry::create();
+		Rtabmap rtabmap;
+		rtabmap.init();
+
+		QApplication app(argc, argv);
+		MapBuilder mapBuilder;
+		mapBuilder.show();
+		QApplication::processEvents();
+
+		SensorData data = camera.takeImage();
+		int cameraIteration = 0;
+		int odometryIteration = 0;
+		printf("Press \"Space\" in the window to pause\n");
+		while(data.isValid() && mapBuilder.isVisible())
+		{
+			if(cameraIteration++ % odomUpdate == 0)
+			{
+				OdometryInfo info;
+				Transform pose = odom->process(data, &info);
+
+				if(odometryIteration++ % mapUpdate == 0)
+				{
+					if(rtabmap.process(data, pose))
+					{
+						mapBuilder.processStatistics(rtabmap.getStatistics());
+						if(rtabmap.getLoopClosureId() > 0)
+						{
+							printf("Loop closure detected!\n");
+						}
+					}
+				}
+
+				mapBuilder.processOdometry(data, pose, info);
+			}
+
+			QApplication::processEvents();
+
+			while(mapBuilder.isPaused() && mapBuilder.isVisible())
+			{
+				uSleep(100);
+				QApplication::processEvents();
+			}
+
+			data = camera.takeImage();
+		}
+		delete odom;
+
+		if(mapBuilder.isVisible())
+		{
+			printf("Processed all frames\n");
+			app.exec();
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	return 0;
+}

rtabmap/examples/RGBDMapping/CMakeLists.txt

@@ -0,0 +1,37 @@
+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 COMPONENTS gui)
+endif()
+
+IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND)
+    SET(moc_srcs MapBuilder.h)
+ENDIF()
+
+ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap::gui)
+
+SET_TARGET_PROPERTIES(
+     rgbd_mapping
+   PROPERTIES
+	 AUTOUIC ON
+     AUTOMOC ON
+     AUTORCC ON
+)
+
+if(internal)
+  SET_TARGET_PROPERTIES( rgbd_mapping 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping)
+endif(internal)

rtabmap/examples/RGBDMapping/MapBuilder.h

@@ -0,0 +1,325 @@
+/*
+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/util3d_mapping.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/global_map/OccupancyGrid.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),
+		grid_(&localGrids_)
+	{
+		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);
+		}
+
+		//============================
+		// Update/add occupancy grid (when RGBD/CreateOccupancyGrid is true)
+		//============================
+		if(grid_.addedNodes().find(stats.getLastSignatureData().id()) == grid_.addedNodes().end())
+		{
+			if(stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
+			{
+				cv::Mat groundCells, obstacleCells, emptyCells;
+				stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
+				localGrids_.add(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells, stats.getLastSignatureData().sensorData().gridCellSize(), stats.getLastSignatureData().sensorData().gridViewPoint());
+			}
+		}
+
+		if(grid_.addedNodes().size() || localGrids_.size())
+		{
+			grid_.update(stats.poses());
+		}
+		if(grid_.addedNodes().size())
+		{
+			float xMin, yMin;
+			cv::Mat map8S = grid_.getMap(xMin, yMin);
+			if(!map8S.empty())
+			{
+				//convert to gray scaled map
+				cv::Mat map8U = util3d::convertMap2Image8U(map8S);
+				cloudViewer_->addOccupancyGridMap(map8U, grid_.getCellSize(), xMin, yMin, 0.75);
+			}
+		}
+
+		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_;
+	LocalGridCache localGrids_;
+	OccupancyGrid grid_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

rtabmap/examples/RGBDMapping/main.cpp

@@ -0,0 +1,297 @@
+/*
+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/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <QApplication>
+#include <stdio.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/filters/filter.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+
+#ifdef RTABMAP_PYTHON
+#include "rtabmap/core/PythonInterface.h"
+#endif
+
+#include "MapBuilder.h"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_mapping driver\n"
+			"  driver       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
+
+	int driver = 0;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		driver = atoi(argv[argc-1]);
+		if(driver < 0 || driver > 10)
+		{
+			UERROR("driver should be between 0 and 10.");
+			showUsage();
+		}
+	}
+
+	// Here is the pipeline that we will use:
+	// CameraOpenni -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// Create the OpenNI camera, it will send a SensorEvent at the rate specified.
+	// Set transform to camera so z is up, y is left and x going forward
+	Camera * camera = 0;
+	if(driver == 1)
+	{
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNI2();
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect();
+	}
+	else if(driver == 3)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV();
+	}
+	else if(driver == 4)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(true);
+	}
+	else if (driver == 5)
+	{
+		if (!CameraFreenect2::available())
+		{
+			UERROR("Not built with Freenect2 support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect2(0, CameraFreenect2::kTypeColor2DepthSD);
+	}
+	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);
+	}
+	else if (driver == 7)
+	{
+		if (!CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense();
+	}
+	else if (driver == 8)
+	{
+		if (!CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 support...");
+			exit(-1);
+		}
+		camera = new CameraRealSense2();
+	}
+	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();
+	}
+
+	if(!camera->init())
+	{
+		UERROR("Camera init failed!");
+	}
+
+	SensorCaptureThread cameraThread(camera);
+
+
+	// 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(Odometry::create());
+
+
+	ParametersMap params;
+	//param.insert(ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // uncomment to create local occupancy grids
+
+	// Create RTAB-Map to process OdometryEvent
+	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->init(params);
+	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
+
+	// Setup handlers
+	odomThread.registerToEventsManager();
+	rtabmapThread.registerToEventsManager();
+	mapBuilder.registerToEventsManager();
+
+	// The RTAB-Map is subscribed by default to SensorEvent, but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent.
+	// We can do that by creating a "pipe" between the camera and odometry, then
+	// only the odometry will receive SensorEvent 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, "SensorEvent");
+
+	// Let's start the threads
+	rtabmapThread.start();
+	odomThread.start();
+	cameraThread.start();
+
+	printf("Press Space key to pause.\n");
+
+	mapBuilder.show();
+	app.exec(); // main loop
+
+	// remove handlers
+	mapBuilder.unregisterFromEventsManager();
+	rtabmapThread.unregisterFromEventsManager();
+	odomThread.unregisterFromEventsManager();
+
+	// Kill all threads
+	cameraThread.kill();
+	odomThread.join(true);
+	rtabmapThread.join(true);
+
+	// Save 3D map
+	printf("Saving rtabmap_cloud.pcd...\n");
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap->getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudRGBFromSensorData(
+				node.sensorData(),
+				4,           // image decimation before creating the clouds
+				4.0f,        // maximum depth of the cloud
+				0.0f,
+				0);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmpNoNaN(new pcl::PointCloud<pcl::PointXYZRGB>);
+		std::vector<int> index;
+		pcl::removeNaNFromPointCloud(*tmp, *tmpNoNaN, index);
+		if(!tmpNoNaN->empty())
+		{
+			*cloud += *util3d::transformPointCloud(tmpNoNaN, iter->second); // transform the point cloud to its pose
+		}
+	}
+	if(cloud->size())
+	{
+		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size());
+		cloud = util3d::voxelize(cloud, 0.01f);
+
+		printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size());
+		pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud);
+		//pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+	}
+	else
+	{
+		printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n");
+	}
+
+	// Save trajectory
+	printf("Saving rtabmap_trajectory.txt ...\n");
+	if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links))
+	{
+		printf("Saving rtabmap_trajectory.txt... done!\n");
+	}
+	else
+	{
+		printf("Saving rtabmap_trajectory.txt... failed!\n");
+	}
+
+	rtabmap->close(false);
+
+	return 0;
+}

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;
+}