feat(slam): add rtabmap_ros

rtabmap/tools/CMakeLists.txt

@@ -0,0 +1,38 @@
+
+ADD_SUBDIRECTORY( ConsoleApp )
+ADD_SUBDIRECTORY( ImagesJoiner )
+ADD_SUBDIRECTORY( ExtractObject )
+ADD_SUBDIRECTORY( Camera )
+ADD_SUBDIRECTORY( StereoEval )
+ADD_SUBDIRECTORY( KittiDataset )
+ADD_SUBDIRECTORY( RgbdDataset )
+ADD_SUBDIRECTORY( EurocDataset )
+ADD_SUBDIRECTORY( Recovery )
+ADD_SUBDIRECTORY( Reprocess )
+ADD_SUBDIRECTORY( DetectMoreLoopClosures )
+ADD_SUBDIRECTORY( Export )
+ADD_SUBDIRECTORY( Report )
+ADD_SUBDIRECTORY( Info )
+ADD_SUBDIRECTORY( CleanupLocalGrids )
+ADD_SUBDIRECTORY( GlobalBundleAdjustment )
+
+IF(OPENCV_NONFREE_FOUND)
+ADD_SUBDIRECTORY( VocabularyComparison )
+ENDIF(OPENCV_NONFREE_FOUND)
+
+IF(TARGET rtabmap_gui)
+	ADD_SUBDIRECTORY( CameraRGBD )
+	IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
+		ADD_SUBDIRECTORY( LidarViewer )
+	ENDIF()
+	ADD_SUBDIRECTORY( DatabaseViewer )
+	ADD_SUBDIRECTORY( EpipolarGeometry )
+	ADD_SUBDIRECTORY( OdometryViewer )
+	ADD_SUBDIRECTORY( DataRecorder )
+	ADD_SUBDIRECTORY( Calibration )
+	ADD_SUBDIRECTORY( Matcher )
+ELSE()
+   MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...")
+ENDIF()
+
+

rtabmap/tools/Calibration/CMakeLists.txt

@@ -0,0 +1,10 @@
+
+ADD_EXECUTABLE(calibration main.cpp)
+TARGET_LINK_LIBRARIES(calibration rtabmap_gui)
+
+SET_TARGET_PROPERTIES( calibration 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-calibration)
+  
+INSTALL(TARGETS calibration
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/Calibration/main.cpp

@@ -0,0 +1,294 @@
+/*
+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/SensorCaptureThread.h>
+#include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/gui/CalibrationDialog.h"
+#include <QApplication>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-calibration [options]\n"
+			"Options:\n"
+			"  --driver #     Driver number to use:-1=USB camera\n"
+			"                                       0=OpenNI-PCL (Kinect)\n"
+			"                                       1=OpenNI2    (Kinect and Xtion PRO Live)\n"
+			"                                       2=Freenect   (Kinect)\n"
+			"                                       3=OpenNI-CV  (Kinect)\n"
+			"                                       4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
+			"                                       5=Freenect2  (Kinect v2)\n"
+			"                                       6=DC1394     (Bumblebee2)\n"
+			"                                       7=FlyCapture2 (Bumblebee2)\n"
+			"                                       11=RealSense2 (T265)\n"
+			"  --device #     Device id\n"
+			"  --debug        Debug log\n"
+			"  --stereo       Stereo: assuming device provides \n"
+			"                 side-by-side stereo images, otherwise \n"
+			"                 add also \"--device_r #\" for the right device.\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setPrintTime(false);
+	ULogger::setPrintWhere(false);
+
+	int driver = -1;
+	int device = 0;
+	int deviceRight = -1;
+	bool stereo = false;
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "--driver") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				driver = std::atoi(argv[i]);
+				if(driver < -1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				device = std::atoi(argv[i]);
+				if(device < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device_r") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				deviceRight = std::atoi(argv[i]);
+				if(deviceRight < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--debug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+			ULogger::setPrintTime(true);
+			ULogger::setPrintWhere(true);
+			continue;
+		}
+		if(strcmp(argv[i], "--stereo") == 0)
+		{
+			stereo=true;
+			continue;
+		}
+		if(strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+	if(driver < -1 || driver > 15)
+	{
+		UERROR("driver should be between -1 and 15.");
+		showUsage();
+	}
+	if(driver == 11)
+	{
+		stereo = true;
+	}
+
+	UINFO("Using driver %d", driver);
+	UINFO("Using device %d", device);
+	UINFO("Stereo: %s", stereo?"true":"false");
+	if(stereo && deviceRight >= 0)
+	{
+		UINFO("Using right device %d", deviceRight);
+	}
+
+	QApplication app(argc, argv);
+	rtabmap::CalibrationDialog dialog(stereo, ".");
+
+	rtabmap::Camera * camera = 0;
+	if(driver == -1)
+	{
+		if(stereo)
+		{
+			if(deviceRight>=0)
+			{
+				// left and right videos
+				camera = new rtabmap::CameraStereoVideo(device, deviceRight);
+			}
+			else
+			{
+				// side-by-side video
+				camera = new rtabmap::CameraStereoVideo(device);
+			}
+		}
+		else
+		{
+			camera = new rtabmap::CameraVideo(device);
+		}
+		dialog.setStereoMode(stereo);
+	}
+	else if(driver == 0)
+	{
+		camera = new rtabmap::CameraOpenni();
+	}
+	else if(driver == 1)
+	{
+		if(!rtabmap::CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNI2();
+	}
+	else if(driver == 2)
+	{
+		if(!rtabmap::CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraFreenect();
+	}
+	else if(driver == 3)
+	{
+		if(!rtabmap::CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNICV(false);
+	}
+	else if(driver == 4)
+	{
+		if(!rtabmap::CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNICV(true);
+	}
+	else if(driver == 5)
+	{
+		if(!rtabmap::CameraFreenect2::available())
+		{
+			UERROR("Not built with Freenect2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColorIR);
+		dialog.setSwitchedImages(true);
+		dialog.setStereoMode(stereo, "rgb", "depth");
+	}
+	else if(driver == 6)
+	{
+		if(!rtabmap::CameraStereoDC1394::available())
+		{
+			UERROR("Not built with DC1394 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoDC1394();
+		dialog.setStereoMode(stereo);
+	}
+	else if(driver == 7)
+	{
+		if(!rtabmap::CameraStereoFlyCapture2::available())
+		{
+			UERROR("Not built with FlyCapture2/Triclops support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoFlyCapture2();
+		dialog.setStereoMode(stereo);
+	}
+	else if(driver == 11)
+	{
+		if(!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2();
+		((rtabmap::CameraRealSense2*)camera)->setImagesRectified(false);
+		dialog.setStereoMode(true);
+		dialog.setFisheyeModel();
+	}
+	else
+	{
+		UFATAL("Calibration for driver %d not available.", driver);
+	}
+
+	rtabmap::SensorCaptureThread * cameraThread = 0;
+
+	if(camera)
+	{
+		if(!camera->init(""))
+		{
+			printf("Camera init failed!\n");
+			delete camera;
+			exit(1);
+		}
+		cameraThread = new rtabmap::SensorCaptureThread(camera);
+	}
+
+	dialog.registerToEventsManager();
+
+	dialog.show();
+	cameraThread->start();
+	app.exec();
+	cameraThread->join(true);
+	delete cameraThread;
+}

rtabmap/tools/Camera/CMakeLists.txt

@@ -0,0 +1,10 @@
+
+ADD_EXECUTABLE(camera main.cpp)
+TARGET_LINK_LIBRARIES(camera rtabmap_core)
+
+SET_TARGET_PROPERTIES( camera 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-camera)
+  
+INSTALL(TARGETS camera
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/Camera/main.cpp

@@ -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.
+*/
+
+#include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/DBReader.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui_c.h>
+#include <stdio.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-camera [option] \n"
+			" Options:\n"
+			"    --device #            USB camera device id (default 0).\n"
+			"    --rate #              Frame rate (default 0 Hz). 0 means as fast as possible.\n"
+			"    --path ""             Path to a directory of images or a video file.\n"
+			"    --calibration ""      Calibration file (*.yaml).\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	int device = 0;
+	std::string path;
+	float rate = 0.0f;
+	std::string calibrationFile;
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "--rate") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = uStr2Float(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--device") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				device = std::atoi(argv[i]);
+				if(device < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--path") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				path = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				calibrationFile = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	if(path.empty())
+	{
+		UINFO("Using device %d", device);
+	}
+	else
+	{
+		UINFO("Using path %s", path.c_str());
+	}
+
+	rtabmap::Camera * camera = 0;
+
+	if(!path.empty())
+	{
+		if(UFile::exists(path))
+		{
+			if(UFile::getExtension(path).compare("db") == 0)
+			{
+				camera = new rtabmap::DBReader(path, rate);
+			}
+			else
+			{
+				camera = new rtabmap::CameraVideo(path, false, rate);
+			}
+		}
+		else if(UDirectory::exists(path))
+		{
+			camera = new rtabmap::CameraImages(path, rate);
+		}
+		else
+		{
+			UERROR("Path not valid! \"%s\"", path.c_str());
+			return -1;
+		}
+	}
+	else
+	{
+		camera = new rtabmap::CameraVideo(device, false, rate);
+	}
+
+	if(camera)
+	{
+		if(!calibrationFile.empty())
+		{
+			UINFO("Set calibration: %s", calibrationFile.c_str());
+		}
+		if(!camera->init(UDirectory::getDir(calibrationFile), UFile::getName(calibrationFile)))
+		{
+			delete camera;
+			UERROR("Cannot initialize the camera.");
+			return -1;
+		}
+	}
+
+	cv::Mat rgb;
+	rgb = camera->takeImage().imageRaw();
+	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
+	while(!rgb.empty())
+	{
+		cv::imshow("Video", rgb); // show frame
+
+		int c = cv::waitKey(10); // wait 10 ms or for key stroke
+		if(c == 27)
+			break; // if ESC, break and quit
+
+		rgb = camera->takeImage().imageRaw();
+	}
+	cv::destroyWindow("Video");
+	if(camera)
+	{
+		delete camera;
+	}
+	return 0;
+}

rtabmap/tools/CameraRGBD/CMakeLists.txt

@@ -0,0 +1,10 @@
+
+ADD_EXECUTABLE(rgbd_camera main.cpp)
+TARGET_LINK_LIBRARIES(rgbd_camera rtabmap_gui)
+
+SET_TARGET_PROPERTIES( rgbd_camera 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_camera)
+  
+INSTALL(TARGETS rgbd_camera
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/CameraRGBD/main.cpp

@@ -0,0 +1,546 @@
+/*
+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/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+#include <pcl/visualization/cloud_viewer.h>
+#include <stdio.h>
+#include <signal.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_camera [options] driver\n"
+			"  driver       Driver number to use: 0=OpenNI-PCL (Kinect)\n"
+			"                                     1=OpenNI2    (Kinect and Xtion PRO Live)\n"
+			"                                     2=Freenect   (Kinect)\n"
+			"                                     3=OpenNI-CV  (Kinect)\n"
+			"                                     4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
+			"                                     5=Freenect2  (Kinect v2)\n"
+			"                                     6=DC1394     (Bumblebee2)\n"
+			"                                     7=FlyCapture2 (Bumblebee2)\n"
+			"                                     8=ZED stereo\n"
+			"                                     9=RealSense\n"
+			"                                     10=Kinect for Windows 2 SDK\n"
+			"                                     11=RealSense2\n"
+			"                                     12=Kinect for Azure SDK\n"
+			"                                     13=MYNT EYE S\n"
+			"                                     14=ZED Open Capture\n"
+			"                                     15=depthai-core\n"
+			"                                     16=XVSDK     (SeerSense)\n"
+			"  Options:\n"
+			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
+			"      -device #                      Device ID (number or string)\n"
+			"      -save_stereo \"path\"            Save stereo images in a folder or a video file (side by side *.avi).\n"
+			"      -fourcc \"XXXX\"               Four characters FourCC code (default is \"MJPG\") used\n"
+			"                                       when saving stereo images to a video file.\n"
+			"                                       See http://www.fourcc.org/codecs.php for more codes.\n");
+	exit(1);
+}
+
+// catch ctrl-c
+bool running = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	running = false;
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+	//ULogger::setPrintTime(false);
+	//ULogger::setPrintWhere(false);
+
+	int driver = 0;
+	std::string stereoSavePath;
+	float rate = 0.0f;
+	std::string fourcc = "MJPG";
+	std::string deviceId;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(strcmp(argv[i], "-rate") == 0)
+			{
+				++i;
+				if(i < argc)
+				{
+					rate = uStr2Float(argv[i]);
+					if(rate < 0.0f)
+					{
+						showUsage();
+					}
+				}
+				else
+				{
+					showUsage();
+				}
+				continue;
+			}
+			if (strcmp(argv[i], "-device") == 0)
+			{
+				++i;
+				if (i < argc)
+				{
+					deviceId = argv[i];
+				}
+				else
+				{
+					showUsage();
+				}
+				continue;
+			}
+			if(strcmp(argv[i], "-save_stereo") == 0)
+			{
+				++i;
+				if(i < argc)
+				{
+					stereoSavePath = argv[i];
+				}
+				else
+				{
+					showUsage();
+				}
+				continue;
+			}
+			if(strcmp(argv[i], "-fourcc") == 0)
+			{
+				++i;
+				if(i < argc)
+				{
+					fourcc = argv[i];
+					if(fourcc.size() != 4)
+					{
+						UERROR("fourcc should be 4 characters.");
+						showUsage();
+					}
+				}
+				else
+				{
+					showUsage();
+				}
+				continue;
+			}
+			if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0)
+			{
+				showUsage();
+			}
+			else if(i< argc-1)
+			{
+				printf("Unrecognized option \"%s\"", argv[i]);
+				showUsage();
+			}
+
+			// last
+			driver = atoi(argv[i]);
+			if(driver < 0 || driver > 15)
+			{
+				UERROR("driver should be between 0 and 15.");
+				showUsage();
+			}
+		}
+	}
+	UINFO("Using driver %d (device=%s)", driver, deviceId.empty()?"0": deviceId.c_str());
+
+	rtabmap::Camera * camera = 0;
+	if(driver < 6)
+	{
+		if(!stereoSavePath.empty())
+		{
+			UWARN("-save_stereo option cannot be used with RGB-D drivers.");
+			stereoSavePath.clear();
+		}
+
+		if(driver == 0)
+		{
+			camera = new rtabmap::CameraOpenni(deviceId);
+		}
+		else if(driver == 1)
+		{
+			if(!rtabmap::CameraOpenNI2::available())
+			{
+				UERROR("Not built with OpenNI2 support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNI2(deviceId);
+		}
+		else if(driver == 2)
+		{
+			if(!rtabmap::CameraFreenect::available())
+			{
+				UERROR("Not built with Freenect support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraFreenect(deviceId.empty()?0:uStr2Int(deviceId));
+		}
+		else if(driver == 3)
+		{
+			if(!rtabmap::CameraOpenNICV::available())
+			{
+				UERROR("Not built with OpenNI from OpenCV support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNICV(false);
+		}
+		else if(driver == 4)
+		{
+			if(!rtabmap::CameraOpenNICV::available())
+			{
+				UERROR("Not built with OpenNI from OpenCV support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraOpenNICV(true);
+		}
+		else if(driver == 5)
+		{
+			if(!rtabmap::CameraFreenect2::available())
+			{
+				UERROR("Not built with Freenect2 support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraFreenect2(deviceId.empty()?0:uStr2Int(deviceId), rtabmap::CameraFreenect2::kTypeColor2DepthSD);
+		}
+	}
+	else if(driver == 6)
+	{
+		if(!rtabmap::CameraStereoDC1394::available())
+		{
+			UERROR("Not built with DC1394 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoDC1394();
+	}
+	else if(driver == 7)
+	{
+		if(!rtabmap::CameraStereoFlyCapture2::available())
+		{
+			UERROR("Not built with FlyCapture2/Triclops support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoFlyCapture2();
+	}
+	else if(driver == 8)
+	{
+		if(!rtabmap::CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED sdk support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZed(deviceId.empty()?0:uStr2Int(deviceId));
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense(deviceId.empty()?0:uStr2Int(deviceId));
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraK4W2::available())
+		{
+			UERROR("Not built with Kinect for Windows 2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4W2(deviceId.empty()?0:uStr2Int(deviceId));
+	}
+	else if (driver == 11)
+	{
+		if (!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2(deviceId);
+	}
+	else if (driver == 12)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A(1);
+	}
+	else if (driver == 13)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye(deviceId);
+	}
+	else if (driver == 14)
+	{
+		if (!rtabmap::CameraStereoZedOC::available())
+		{
+			UERROR("Not built with Zed Open Capture support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZedOC(deviceId.empty()?-1:uStr2Int(deviceId));
+	}
+	else if (driver == 15)
+	{
+		if (!rtabmap::CameraDepthAI::available())
+		{
+			UERROR("Not built with depthai-core support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraDepthAI(deviceId);
+	}
+	else if (driver == 16)
+	{
+		if (!rtabmap::CameraSeerSense::available())
+		{
+			UERROR("Not built with XVisio SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraSeerSense();
+	}
+	else
+	{
+		UFATAL("");
+	}
+
+	if(!camera->init())
+	{
+		printf("Camera init failed! Please select another driver (see \"--help\").\n");
+		delete camera;
+		exit(1);
+	}
+
+	rtabmap::SensorData data = camera->takeData();
+	if (data.imageRaw().empty())
+	{
+		printf("Cloud not get frame from the camera!\n");
+		delete camera;
+		exit(1);
+	}
+	if(data.imageRaw().cols % data.depthOrRightRaw().cols != 0 || data.imageRaw().rows % data.depthOrRightRaw().rows != 0)
+	{
+		UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
+				data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
+	}
+	pcl::visualization::CloudViewer * viewer = 0;
+	if((data.stereoCameraModels().empty() || data.stereoCameraModels()[0].isValidForProjection()) &&
+	   (data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
+	{
+		UWARN("Camera not calibrated! The registered cloud cannot be shown.");
+	}
+	else
+	{
+		viewer = new pcl::visualization::CloudViewer("cloud");
+	}
+
+	cv::VideoWriter videoWriter;
+	UDirectory dir;
+	if(!stereoSavePath.empty() &&
+	   !data.imageRaw().empty() &&
+	   !data.rightRaw().empty())
+	{
+		if(UFile::getExtension(stereoSavePath).compare("avi") == 0)
+		{
+			if(data.imageRaw().size() == data.rightRaw().size())
+			{
+				if(rate <= 0)
+				{
+					UERROR("You should set the input rate when saving stereo images to a video file.");
+					showUsage();
+				}
+				cv::Size targetSize = data.imageRaw().size();
+				targetSize.width *= 2;
+				UASSERT(fourcc.size() == 4);
+				videoWriter.open(
+						stereoSavePath,
+						CV_FOURCC(fourcc.at(0), fourcc.at(1), fourcc.at(2), fourcc.at(3)),
+						rate,
+						targetSize,
+						data.imageRaw().channels() == 3);
+			}
+			else
+			{
+				UERROR("Images not the same size, cannot save stereo images to the video file.");
+			}
+		}
+		else if(UDirectory::exists(stereoSavePath))
+		{
+			UDirectory::makeDir(stereoSavePath+"/"+"left");
+			UDirectory::makeDir(stereoSavePath+"/"+"right");
+		}
+		else
+		{
+			UERROR("Directory \"%s\" doesn't exist.", stereoSavePath.c_str());
+			stereoSavePath.clear();
+		}
+	}
+
+	// to catch the ctrl-c
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	int id=1;
+	while(!data.imageRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
+	{
+		cv::Mat rgb = data.imageRaw();
+		if(!data.depthRaw().empty() && (data.depthRaw().type() == CV_16UC1 || data.depthRaw().type() == CV_32FC1))
+		{
+			// depth
+			cv::Mat depth = data.depthRaw();
+			if(depth.type() == CV_32FC1)
+			{
+				depth = rtabmap::util2d::cvtDepthFromFloat(depth);
+			}
+
+			if(!rgb.empty() && rgb.cols % depth.cols == 0 && rgb.rows % depth.rows == 0 &&
+					data.cameraModels().size() &&
+					data.cameraModels()[0].isValidForProjection())
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(
+						rgb, depth,
+						data.cameraModels()[0]);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
+				if(viewer)
+					viewer->showCloud(cloud, "cloud");
+			}
+			else if(!depth.empty() &&
+					data.cameraModels().size() &&
+					data.cameraModels()[0].isValidForProjection())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(
+						depth,
+						data.cameraModels()[0]);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.cameraModels()[0].localTransform());
+				viewer->showCloud(cloud, "cloud");
+			}
+
+			cv::Mat tmp;
+			unsigned short min=0, max = 2048;
+			uMinMax((unsigned short*)depth.data, depth.rows*depth.cols, min, max);
+			depth.convertTo(tmp, CV_8UC1, 255.0/max);
+
+			cv::imshow("Video", rgb); // show frame
+			cv::imshow("Depth", tmp);
+		}
+		else if(!data.rightRaw().empty())
+		{
+			// stereo
+			cv::Mat right = data.rightRaw();
+			cv::imshow("Left", rgb); // show frame
+			cv::imshow("Right", right);
+
+			if(rgb.cols == right.cols && rgb.rows == right.rows && data.stereoCameraModels().size()==1 && data.stereoCameraModels()[0].isValidForProjection())
+			{
+				if(right.channels() == 3)
+				{
+					cv::cvtColor(right, right, CV_BGR2GRAY);
+				}
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromStereoImages(
+						rgb, right,
+						data.stereoCameraModels()[0]);
+				cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform::opengl_T_rtabmap()*data.stereoCameraModels()[0].localTransform());
+				if(viewer)
+					viewer->showCloud(cloud, "cloud");
+			}
+		}
+
+		int c = cv::waitKey(10); // wait 10 ms or for key stroke
+		if(c == 27)
+			break; // if ESC, break and quit
+
+		if(videoWriter.isOpened())
+		{
+			cv::Mat left = data.imageRaw();
+			cv::Mat right = data.rightRaw();
+			if(left.size() == right.size())
+			{
+				cv::Size targetSize = left.size();
+				targetSize.width *= 2;
+				cv::Mat targetImage(targetSize, left.type());
+				if(right.type() != left.type())
+				{
+					cv::Mat tmp;
+					cv::cvtColor(right, tmp, left.channels()==3?CV_GRAY2BGR:CV_BGR2GRAY);
+					right = tmp;
+				}
+				UASSERT(left.type() == right.type());
+
+				cv::Mat roiA(targetImage, cv::Rect( 0, 0, left.size().width, left.size().height ));
+				left.copyTo(roiA);
+				cv::Mat roiB( targetImage, cvRect( left.size().width, 0, left.size().width, left.size().height ) );
+				right.copyTo(roiB);
+
+				videoWriter.write(targetImage);
+				printf("Saved frame %d to \"%s\"\n", id, stereoSavePath.c_str());
+			}
+			else
+			{
+				UERROR("Left and right images are not the same size!?");
+			}
+		}
+		else if(!stereoSavePath.empty())
+		{
+			cv::imwrite(stereoSavePath+"/"+"left/"+uNumber2Str(id) + ".jpg", data.imageRaw());
+			cv::imwrite(stereoSavePath+"/"+"right/"+uNumber2Str(id) + ".jpg", data.rightRaw());
+			printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str());
+		}
+		++id;
+		data = camera->takeData();
+	}
+	printf("Closing...\n");
+	if(viewer)
+	{
+		delete viewer;
+	}
+	cv::destroyWindow("Video");
+	cv::destroyWindow("Depth");
+	delete camera;
+	return 0;
+}

rtabmap/tools/CleanupLocalGrids/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(cleanupLocalGrids main.cpp)
+  
+TARGET_LINK_LIBRARIES(cleanupLocalGrids rtabmap_core)
+
+SET_TARGET_PROPERTIES( cleanupLocalGrids 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-cleanupLocalGrids)
+
+INSTALL(TARGETS cleanupLocalGrids
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

rtabmap/tools/CleanupLocalGrids/main.cpp

@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2010-2021, 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/Memory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/util3d_transforms.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\n"
+			"Clear empty space from local occupancy grids and laser scans based on the saved optimized global 2d grid map.\n"
+			"Advantages:\n"
+			"   * If the map needs to be regenerated in the future (e.g., when \n"
+			"     we re-use the map in SLAM mode), removed obstacles won't reappear.\n"
+			"   * [--scan] The cropped laser scans will be also used for localization,\n"
+			"     so if dynamic obstacles have been removed, localization won't try to\n"
+			"     match them anymore.\n\n"
+			"Disadvantage:\n"
+			"   * [--scan] Cropping the laser scans cannot be reverted, but grids can.\n"
+			"\nUsage:\n"
+			"rtabmap-cleanupLocalGrids [options] database.db\n"
+			"Options:\n"
+			"    --radius #   Radius in cells around empty cell without obstacles to clear\n"
+			"                  underlying obstacles. Default is 1.\n"
+			"    --scan       Filter also scans, otherwise only local grids are filtered.\n"
+			"\n");
+	;
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	int cropRadius = 1;
+	bool filterScans = false;
+
+	for(int i=1; i<argc; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+		else if(std::strcmp(argv[i], "--scan") == 0)
+		{
+			filterScans = true;
+		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				cropRadius = uStr2Int(argv[i]);
+				UASSERT(cropRadius>=0);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+	}
+
+	std::string dbPath = argv[argc-1];
+
+	if(!UFile::exists(dbPath))
+	{
+		UERROR("File \"%s\" doesn't exist!", dbPath.c_str());
+		return -1;
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	Rtabmap rtabmap;
+	rtabmap.init(ParametersMap(), dbPath, true);
+
+	float xMin, yMin, cellSize;
+	cv::Mat map = rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize);
+	if(map.empty())
+	{
+		UERROR("Database %s doesn't have optimized 2d map saved in it!", dbPath.c_str());
+		return -1;
+	}
+
+	printf("Options:\n");
+	printf("  --radius:  %d cell(s) (cell size=%.3fm)\n", cropRadius, cellSize);
+	printf("  --scan: %s\n", filterScans?"true":"false");
+
+	std::map<int, Transform> poses = rtabmap.getLocalOptimizedPoses();
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Database %s doesn't have optimized poses saved in it!", dbPath.c_str());
+		return -1;
+	}
+
+	UTimer timer;
+	printf("Cleaning grids...\n");
+	int modifiedCells = rtabmap.cleanupLocalGrids(poses, map, xMin, yMin, cellSize, cropRadius, filterScans);
+	printf("Cleanup %d cells! (%fs)\n", modifiedCells, timer.ticks());
+
+	rtabmap.close();
+
+	printf("Done!\n");
+	return 0;
+}

rtabmap/tools/ConsoleApp/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+# Add binary called "consoleApp" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(consoleApp main.cpp)
+TARGET_LINK_LIBRARIES(consoleApp rtabmap_core)
+
+SET_TARGET_PROPERTIES( consoleApp 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console)
+  
+INSTALL(TARGETS consoleApp
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+

rtabmap/tools/ConsoleApp/main.cpp

@@ -0,0 +1,570 @@
+/*
+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/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraRGB.h"
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <fstream>
+#include <queue>
+#include <opencv2/core/core.hpp>
+#include <signal.h>
+
+using namespace rtabmap;
+
+#define GENERATED_GT_NAME "GroundTruth_generated.bmp"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-console [options] \"path\"\n"
+			"  path                            For images, use the directory path. For videos or databases, use full\n "
+			"                                  path name\n"
+			"Options:\n"
+			"  -quiet                          Don't show log for every images.\n"
+			"  -rate #.##                      Acquisition time (seconds)\n"
+			"  -rateHz #.##                    Acquisition rate (Hz), for convenience\n"
+			"  -repeat #                       Repeat the process on the data set # times (minimum of 1)\n"
+			"  -createGT                       Generate a ground truth file\n"
+			"  -gt \"path\"                      Compute precision/recall with ground truth matrix.\n"
+			"  -start_at #                     When \"path\" is a directory of images, set this parameter\n"
+			"                                   to start processing at image # (default 0).\n"
+			"  -skip #                         Skip X images while reading directory (default 0).\n"
+			"  -v                              Get version of RTAB-Map\n"
+			"  -input \"path\"                   Load previous database if it exists.\n"
+			"%s\n"
+			"Example (generating LogI.txt and LogF.txt for rtabmap/archive/2010-LoopClosure/ShowLogs script, and with 2013 paper parameters):\n\n"
+			"   $ rtabmap-console \\\n"
+			"       --Rtabmap/StatisticLogged true\\\n"
+			"       --Rtabmap/StatisticLoggedHeaders false\\\n"
+			"       --Kp/DetectorStrategy 0\\\n"
+			"       --SURF/HessianThreshold 150\\\n"
+			"       --Rtabmap/MemoryThr 300\\\n"
+			"       --Rtabmap/LoopRatio 0.9\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       --Vis/MaxFeatures 400\\\n"
+			"       --Kp/TfIdfLikelihoodUsed false\\\n"
+			"       --Kp/MaxFeatures 400\\\n"
+			"       --Kp/BadSignRatio 0.25\\\n"
+			"       --Mem/BadSignaturesIgnored true\\\n"
+			"       --Mem/RehearsalSimilarity 0.20\\\n"
+			"       --Mem/RecentWmRatio 0.20\\\n"
+			"       -gt \"~/Downloads/UdeS_1Hz.png\"\\\n"
+			"       ~/Downloads/UdeS_1Hz\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap pm = Parameters::parseArguments(argc, argv);
+	pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else if(argc == 2 && strcmp(argv[1], "-v") == 0)
+	{
+		printf("%s\n", Parameters::getVersion().c_str());
+		exit(0);
+	}
+	printf("\n");
+
+	std::string path;
+	float rate = 0.0;
+	int loopDataset = 0;
+	int repeat = 0;
+	bool createGT = false;
+	std::string inputDbPath;
+	std::string gtPath;
+	int startAt = 0;
+	int skip = 0;
+	bool quiet = false;
+
+	for(int i=1; i<argc; ++i)
+	{
+		if(i == argc-1)
+		{
+			// The last must be the path
+			path = argv[i];
+			if(!UDirectory::exists(path.c_str()) && !UFile::exists(path.c_str()))
+			{
+				printf("Path not valid : %s\n", path.c_str());
+				showUsage();
+				exit(1);
+			}
+			break;
+		}
+		if(strcmp(argv[i], "-rate") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = uStr2Float(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-rateHz") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = uStr2Float(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+				else if(rate)
+				{
+					rate = 1/rate;
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-repeat") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				repeat = std::atoi(argv[i]);
+				if(repeat < 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-start_at") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				startAt = std::atoi(argv[i]);
+				if(startAt < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if (strcmp(argv[i], "-skip") == 0)
+		{
+			++i;
+			if (i < argc)
+			{
+				skip = std::atoi(argv[i]);
+				if (skip < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-quiet") == 0)
+		{
+			quiet = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-createGT") == 0)
+		{
+			createGT = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-gt") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				gtPath = uReplaceChar(argv[i], '~', UDirectory::homeDir());
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-input") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				inputDbPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+
+	if(repeat && createGT)
+	{
+		printf("Cannot create a Ground truth if repeat is on.\n");
+		showUsage();
+	}
+
+	UTimer timer;
+	timer.start();
+	std::queue<double> iterationMeanTime;
+
+	Camera * camera = 0;
+	int totalImages = 0;
+	if(UDirectory::exists(path))
+	{
+		camera = new CameraImages(path, rate>0.0f?1.0f/rate:0.0f);
+		((CameraImages*)camera)->setStartIndex(startAt);
+	}
+	else
+	{
+		camera = new CameraVideo(path, false, rate>0.0f?1.0f/rate:0.0f);
+	}
+
+	if(!camera || !camera->init())
+	{
+		printf("Camera init failed, using path \"%s\"\n", path.c_str());
+		exit(1);
+	}
+
+	if(dynamic_cast<CameraImages*>(camera))
+		totalImages = ((CameraImages*)camera)->imagesCount();
+
+	std::map<int, int> generatedGroundTruth;
+
+	// Create tasks
+	Rtabmap rtabmap;
+	if(inputDbPath.empty())
+	{
+		inputDbPath = "rtabmapconsole.db";
+		if(UFile::erase(inputDbPath) == 0)
+		{
+			printf("Deleted database \"%s\".\n", inputDbPath.c_str());
+		}
+	}
+	else
+	{
+		printf("Loading database \"%s\".\n", inputDbPath.c_str());
+	}
+
+	// Disable RGB-D mode
+	uInsert(pm, ParametersPair(Parameters::kRGBDEnabled(), "false"));
+
+	// Process an empty image to make sure every libraries are loaded.
+	ULogger::Level level = ULogger::level();
+	ULogger::setLevel(ULogger::kError);
+	cv::Mat tmp = cv::Mat::zeros(640,480,CV_8UC1);
+	rtabmap.init(pm);
+	rtabmap.process(tmp);
+	rtabmap.close(false);
+	ULogger::setLevel(level);
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	rtabmap.init(pm, inputDbPath);
+
+	printf("rtabmap init time = %fs\n", timer.ticks());
+
+	// Start thread's task
+	int loopClosureId;
+	int count = 0;
+	int countLoopDetected=0;
+	printf("\nParameters : \n");
+	printf(" Data set : %s\n", path.c_str());
+	printf(" Time threshold = %1.2f ms\n", rtabmap.getTimeThreshold());
+	printf(" Memory threshold = %d nodes\n", rtabmap.getMemoryThreshold());
+	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
+	printf(" Repeating data set = %s\n", repeat?"true":"false");
+	printf(" Camera starts at image %d (default 0)\n", startAt);
+	printf(" Skip image = %d\n", skip);
+	cv::Mat inputGT;
+	if(!gtPath.empty())
+	{
+		if(startAt != 0 || repeat || skip>0)
+		{
+			printf(" Cannot input ground truth if startAt,repeat,skip options are used.\n");
+			gtPath.clear();
+		}
+		inputGT = cv::imread(gtPath, cv::IMREAD_GRAYSCALE);
+		printf(" Input ground truth : %s (%dx%d)\n", gtPath.c_str(), inputGT.cols, inputGT.rows);
+		UASSERT(inputGT.cols == inputGT.rows);
+		UASSERT(totalImages == 0 || totalImages == inputGT.cols);
+	}
+	if(createGT)
+	{
+		printf(" Creating the ground truth matrix.\n");
+	}
+	printf(" INFO: All other parameters are set to defaults\n");
+	if(pm.size()>1)
+	{
+		printf("   Overwritten parameters :\n");
+		for(ParametersMap::iterator iter = pm.begin(); iter!=pm.end(); ++iter)
+		{
+			printf("    %s=%s\n",iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	if(rtabmap.getWM().size() || rtabmap.getSTM().size())
+	{
+		printf("[Warning] RTAB-Map database is not empty (%s)\n", inputDbPath.c_str());
+	}
+	printf("\nProcessing images...\n");
+
+	UTimer iterationTimer;
+	UTimer rtabmapTimer;
+	int imagesProcessed = 0;
+	std::list<std::vector<float> > teleopActions;
+	std::map<float, bool> loopClosureStats;
+	while(loopDataset <= repeat && g_forever)
+	{
+		SensorData data = camera->takeImage();
+		int i=0;
+		double maxIterationTime = 0.0;
+		int maxIterationTimeId = 0;
+		while(!data.imageRaw().empty() && g_forever)
+		{
+			++imagesProcessed;
+			iterationTimer.start();
+			rtabmapTimer.start();
+			rtabmap.process(data.imageRaw());
+			double rtabmapTime = rtabmapTimer.elapsed();
+			loopClosureId = rtabmap.getLoopClosureId();
+			if(loopClosureId)
+			{
+				++countLoopDetected;
+			}
+
+			if(!gtPath.empty() && rtabmap.getHighestHypothesisValue() > 0.0f)
+			{
+				if(i>inputGT.rows ||
+				   rtabmap.getHighestHypothesisId()-1 > inputGT.cols)
+				{
+					printf("ERROR: Incompatible ground truth file (size=%dx%d, current image index=%d, loop index=%d)!", inputGT.cols, inputGT.rows, i, rtabmap.getHighestHypothesisId()-1);
+					exit(1);
+				}
+				bool rejectedHypothesis = uValue(rtabmap.getStatistics().data(), Statistics::kLoopRejectedHypothesis(), 0.0f) != 0.0f;
+				unsigned char gtValue = inputGT.at<unsigned char>(i, rtabmap.getHighestHypothesisId()-1);
+				if((gtValue==0 || gtValue == 255) && !rejectedHypothesis)
+				{
+					loopClosureStats.insert(std::make_pair(rtabmap.getHighestHypothesisValue(), gtValue==255));
+				}
+			}
+
+			for(int j=0; j<=skip; ++j)
+			{
+				data = camera->takeImage();
+			}
+			if(!quiet && ++count % 100 == 0)
+			{
+				printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",
+						count, countLoopDetected, maxIterationTimeId, maxIterationTime);
+				maxIterationTime = 0.0;
+				maxIterationTimeId = 0;
+				std::map<int, int> wm = rtabmap.getWeights();
+				printf(" WM(%d)=[", (int)wm.size());
+				for(std::map<int, int>::iterator iter=wm.begin(); iter!=wm.end();++iter)
+				{
+					if(iter != wm.begin())
+					{
+						printf(";");
+					}
+					printf("%d,%d", iter->first, iter->second);
+				}
+				printf("]\n");
+			}
+
+			// Update generated ground truth matrix
+			if(createGT)
+			{
+				if(loopClosureId > 0)
+				{
+					generatedGroundTruth.insert(std::make_pair(i, loopClosureId-1));
+				}
+			}
+
+			++i;
+
+			double iterationTime = iterationTimer.ticks();
+
+			if(rtabmapTime > maxIterationTime)
+			{
+				maxIterationTime = rtabmapTime;
+				maxIterationTimeId = count;
+			}
+
+			ULogger::flush();
+
+			if(!quiet)
+			{
+				if(rtabmap.getLoopClosureId())
+				{
+					printf(" iteration(%d) loop(%d) hyp(%.2f) time=%fs/%fs *\n",
+							count, rtabmap.getLoopClosureId(), rtabmap.getLoopClosureValue(), rtabmapTime, iterationTime);
+				}
+				else if(rtabmap.getHighestHypothesisId())
+				{
+					printf(" iteration(%d) high(%d) hyp(%.2f) time=%fs/%fs\n",
+							count, rtabmap.getHighestHypothesisId(), rtabmap.getHighestHypothesisValue(), rtabmapTime, iterationTime);
+				}
+				else
+				{
+					printf(" iteration(%d) time=%fs/%fs\n", count, rtabmapTime, iterationTime);
+				}
+
+				if(rtabmap.getTimeThreshold() && rtabmapTime > rtabmap.getTimeThreshold()*100.0f)
+				{
+					printf(" ERROR,  there is  problem, too much time taken... %fs", rtabmapTime);
+					break; // there is  problem, don't continue
+				}
+			}
+			else if(totalImages>0 && i % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+		}
+		++loopDataset;
+		if(loopDataset <= repeat)
+		{
+			camera->init();
+			printf(" Beginning loop %d...\n", loopDataset);
+		}
+	}
+	printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
+	printf(" Total time = %fs\n", timer.ticks());
+
+	if(!loopClosureStats.empty())
+	{
+		int totalGoodLoopClosures = 0;
+		float loopThr = 0.0f;
+		for(std::map<float, bool>::reverse_iterator iter=loopClosureStats.rbegin(); iter!=loopClosureStats.rend(); ++iter)
+		{
+			if(!iter->second)
+			{
+				break;
+			}
+			loopThr = iter->first;
+			++totalGoodLoopClosures;
+		}
+		int totalGtLoopClosures = 0;
+		for(int i=0; i<inputGT.rows; ++i)
+		{
+			for(int j=0; j<inputGT.cols; ++j)
+			{
+				if(inputGT.at<unsigned char>(i,j) == 255)
+				{
+					++totalGtLoopClosures;
+					break;
+				}
+			}
+		}
+
+		printf(" Recall (100%% Precision): %.2f%% (with %s=%f, accepted=%d/%d)\n",
+				float(totalGoodLoopClosures)/float(totalGtLoopClosures)*100.0f,
+				Parameters::kRtabmapLoopThr().c_str(),
+				loopThr,
+				totalGoodLoopClosures,
+				totalGtLoopClosures);
+	}
+
+	if(imagesProcessed && createGT)
+	{
+		cv::Mat generatedGroundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
+
+		for(std::map<int, int>::iterator iter = generatedGroundTruth.begin(); iter!=generatedGroundTruth.end(); ++iter)
+		{
+			generatedGroundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
+		}
+
+		// Generate the ground truth file
+		printf("Generate ground truth to file %s, size of %d\n", GENERATED_GT_NAME, generatedGroundTruthMat.rows);
+		cv::imwrite(GENERATED_GT_NAME, generatedGroundTruthMat);
+		printf(" Creating ground truth file = %fs\n", timer.ticks());
+	}
+
+	if(camera)
+	{
+		delete camera;
+		camera = 0 ;
+	}
+
+	rtabmap.close();
+
+	printf(" Cleanup time = %fs\n", timer.ticks());
+
+	printf("Database (\"%s\") and log files saved to current directory.\n", inputDbPath.c_str());
+
+	return 0;
+}

rtabmap/tools/DataRecorder/CMakeLists.txt

@@ -0,0 +1,14 @@
+
+IF(MINGW)
+  ADD_EXECUTABLE(dataRecorder WIN32 main.cpp)
+ELSE()
+  ADD_EXECUTABLE(dataRecorder main.cpp)
+ENDIF()
+TARGET_LINK_LIBRARIES(dataRecorder rtabmap_gui)
+
+SET_TARGET_PROPERTIES( dataRecorder 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-dataRecorder)
+
+INSTALL(TARGETS dataRecorder
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/DataRecorder/main.cpp

@@ -0,0 +1,248 @@
+/*
+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/utilite/ULogger.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/CameraRGBD.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/SensorEvent.h>
+#include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <rtabmap/gui/DataRecorder.h>
+#include <rtabmap/gui/PreferencesDialog.h>
+#include <QApplication>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"dataRecorder [options] config.ini output.db\n"
+			"Description:\n"
+			"  A config file contains all camera parameters and driver used. That\n"
+			"  file can be generated by RTAB-Map->Preferences->Save Settings(*.ini)\n"
+			"  after modifying Source settings.\n"
+			"Options:\n"
+			"  -debug    Show debug log.\n"
+			"  -hide     Don't display the current cloud recorded.\n");
+	exit(1);
+}
+
+rtabmap::SensorCaptureThread * cam = 0;
+QApplication * app = 0;
+// catch ctrl-c
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	if(cam)
+	{
+		cam->join(true);
+	}
+	if(app)
+	{
+		QMetaObject::invokeMethod(app, "quit");
+	}
+}
+
+// Detect when we reached end-of-files
+class StatusHandler: public UEventsHandler{
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("SensorEvent") == 0)
+		{
+			SensorEvent * camEvent = (SensorEvent*)event;
+			if(camEvent->getCode() == SensorEvent::kCodeNoMoreImages)
+			{
+				printf("End of stream reached...\n");
+				if(cam)
+				{
+					cam->join(true);
+				}
+				if(app)
+				{
+					QMetaObject::invokeMethod(app, "quit");
+				}
+			}
+		}
+		return false;
+	}
+};
+
+int main (int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	// parse arguments
+	std::string fileName;
+	bool show = true;
+	std::string configFile;
+
+	if(argc < 3)
+	{
+		showUsage();
+	}
+	for(int i=1; i<argc-2; ++i)
+	{
+		if(strcmp(argv[i], "-debug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+			continue;
+		}
+		if(strcmp(argv[i], "-hide") == 0)
+		{
+			show = false;
+			continue;
+		}
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+	configFile = argv[argc-2];
+	configFile = uReplaceChar(configFile, '~', UDirectory::homeDir());
+	fileName = argv[argc-1]; // the last is the output path
+	fileName = uReplaceChar(fileName, '~', UDirectory::homeDir());
+
+	if(UFile::getExtension(fileName).compare("db") != 0)
+	{
+		printf("Database names must end with .db extension\n");
+		showUsage();
+	}
+
+	UINFO("Output = %s", fileName.c_str());
+	UINFO("Show = %s", show?"true":"false");
+	UINFO("Config = %s", configFile.c_str());
+
+	app = new QApplication(argc, argv);
+
+	PreferencesDialog dialog;
+	//Set working directory to default if not in config file to avoid message box
+	ParametersMap paramTmp;
+	Parameters::readINI(configFile, paramTmp);
+	if(paramTmp.find(Parameters::kRtabmapWorkingDirectory()) == paramTmp.end())
+	{
+		paramTmp.clear();
+		paramTmp.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), dialog.getDefaultWorkingDirectory().toStdString()));
+		Parameters::writeINI(configFile, paramTmp);
+	}
+	dialog.init(configFile.c_str());
+
+	UINFO("Driver = %d", dialog.getSourceDriver());
+	UINFO("Rate   = %f Hz", dialog.getGeneralInputRate());
+
+	// Catch ctrl-c to close the gui
+	// (Place this after QApplication's constructor)
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	// Catch end of stream to close the gui
+	StatusHandler statusHandler;
+	statusHandler.registerToEventsManager();
+
+	rtabmap::Camera * camera = dialog.createCamera();
+	if(camera == 0)
+	{
+		return -1;
+	}
+	ParametersMap parameters = dialog.getAllParameters();
+	cam = new SensorCaptureThread(camera, parameters);
+	cam->setMirroringEnabled(dialog.isSourceMirroring());
+	cam->setColorOnly(dialog.isSourceRGBDColorOnly());
+	cam->setImageDecimation(dialog.getSourceImageDecimation());
+	cam->setHistogramMethod(dialog.getSourceHistogramMethod());
+	cam->setStereoToDepth(dialog.isSourceStereoDepthGenerated());
+	cam->setStereoExposureCompensation(dialog.isSourceStereoExposureCompensation());
+	cam->setScanParameters(
+			dialog.isSourceScanFromDepth(),
+			dialog.getSourceScanDownsampleStep(),
+			dialog.getSourceScanRangeMin(),
+			dialog.getSourceScanRangeMax(),
+			dialog.getSourceScanVoxelSize(),
+			dialog.getSourceScanNormalsK(),
+			dialog.getSourceScanNormalsRadius(),
+			(float)dialog.getSourceScanForceGroundNormalsUp());
+	if(dialog.getIMUFilteringStrategy()>0 && dynamic_cast<DBReader*>(camera) == 0)
+	{
+		cam->enableIMUFiltering(dialog.getIMUFilteringStrategy()-1, parameters, dialog.getIMUFilteringBaseFrameConversion());
+	}
+	if(dialog.isDepthFilteringAvailable())
+	{
+		if(dialog.isBilateralFiltering())
+		{
+			cam->enableBilateralFiltering(
+					dialog.getBilateralSigmaS(),
+					dialog.getBilateralSigmaR());
+		}
+		cam->setDistortionModel(dialog.getSourceDistortionModel().toStdString());
+	}
+
+	DataRecorder recorder;
+
+	if(recorder.init(fileName.c_str()))
+	{
+		recorder.registerToEventsManager();
+		if(show)
+		{
+			recorder.setWindowTitle("Data recorder");
+			recorder.setMinimumWidth(500);
+			recorder.setMinimumHeight(300);
+			recorder.showNormal();
+			app->processEvents();
+		}
+
+		if(camera->init())
+		{
+			cam->start();
+
+			app->exec();
+
+			UINFO("Closing...");
+
+			recorder.close();
+		}
+		else
+		{
+			UERROR("Cannot initialize the camera!");
+		}
+	}
+	else
+	{
+		UERROR("Cannot initialize the recorder! Maybe the path is wrong: \"%s\"", fileName.c_str());
+	}
+
+	if(cam)
+	{
+		delete cam;
+	}
+
+	return 0;
+}

rtabmap/tools/DatabaseViewer/CMakeLists.txt

@@ -0,0 +1,15 @@
+
+IF(MINGW)
+  ADD_EXECUTABLE(databaseViewer WIN32 main.cpp)
+ELSE()
+  ADD_EXECUTABLE(databaseViewer main.cpp)
+ENDIF()
+
+TARGET_LINK_LIBRARIES(databaseViewer rtabmap_gui)
+
+SET_TARGET_PROPERTIES( databaseViewer 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer)
+
+INSTALL(TARGETS databaseViewer
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/DatabaseViewer/main.cpp

@@ -0,0 +1,80 @@
+/*
+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 <QApplication>
+#include "rtabmap/gui/DatabaseViewer.h"
+#include "rtabmap/utilite/ULogger.h"
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
+
+#include <vtkObject.h>
+#include <vtkVersionMacros.h>
+
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+#include <QVTKRenderWidget.h>
+#endif
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+#ifdef WIN32
+	CoInitialize(nullptr);
+#endif
+
+#if VTK_MAJOR_VERSION > 9 || (VTK_MAJOR_VERSION==9 && VTK_MINOR_VERSION >= 1)
+    // needed to ensure appropriate OpenGL context is created for VTK rendering.
+    QSurfaceFormat::setDefaultFormat(QVTKRenderWidget::defaultFormat());
+#endif
+
+#ifdef RTABMAP_PYTHON
+	rtabmap::PythonInterface pythonInterface;
+#endif
+
+	QApplication * app = new QApplication(argc, argv);
+	rtabmap::DatabaseViewer * mainWindow = new rtabmap::DatabaseViewer();
+
+	mainWindow->showNormal();
+
+	if(argc >= 2)
+	{
+		mainWindow->openDatabase(argv[argc-1], rtabmap::Parameters::parseArguments(argc, argv, true));
+	}
+
+	// Now wait for application to finish
+	app->connect( app, SIGNAL( lastWindowClosed() ),
+				app, SLOT( quit() ) );
+	app->exec();// MUST be called by the Main Thread
+
+	delete mainWindow;
+	delete app;
+	UINFO("All done! closing...");
+
+	return 0;
+}

rtabmap/tools/DetectMoreLoopClosures/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(detectMoreLoopClosures main.cpp)
+  
+TARGET_LINK_LIBRARIES(detectMoreLoopClosures rtabmap_core)
+
+SET_TARGET_PROPERTIES( detectMoreLoopClosures 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-detectMoreLoopClosures)
+
+INSTALL(TARGETS detectMoreLoopClosures
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

rtabmap/tools/DetectMoreLoopClosures/main.cpp

@@ -0,0 +1,272 @@
+/*
+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/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-detectMoreLoopClosures [options] database.db\n"
+			"Options:\n"
+			"    -r #          Cluster radius (default 1 m).\n"
+			"    -rx #         Cluster radius min (default 0 m).\n"
+			"    -a #          Cluster angle (default 30 deg).\n"
+			"    -i #          Iterations (default 1).\n"
+			"    --intra       Add only intra-session loop closures.\n"
+			"    --inter       Add only inter-session loop closures.\n"
+			"\n%s", Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_loopForever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_loopForever = false;
+}
+
+class PrintProgressState : public ProgressState
+{
+public:
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			printf("%s \n", msg.c_str());
+		return g_loopForever;
+	}
+};
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	float clusterRadiusMin = 0.0f;
+	float clusterRadiusMax = 1.0f;
+	float clusterAngle = CV_PI/6.0f;
+	int iterations = 1;
+	bool intraSession = false;
+	bool interSession = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+		else if(std::strcmp(argv[i], "--intra") == 0)
+		{
+			intraSession = true;
+			if(interSession)
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--inter") == 0)
+		{
+			interSession = true;
+			if(intraSession)
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-r") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				clusterRadiusMax = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-rx") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				clusterRadiusMin = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-a") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				clusterAngle = uStr2Float(argv[i])*CV_PI/180.0f;
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-i") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				iterations = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+	}
+	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+	}
+
+	printf("\nDatabase: %s\n", dbPath.c_str());
+	printf("Cluster radius min = %f m\n", clusterRadiusMin);
+	printf("Cluster radius max = %f m\n", clusterRadiusMax);
+	printf("Cluster angle = %f deg\n", clusterAngle*180.0f/CV_PI);
+	if(intraSession)
+	{
+		printf("Intra-session only\n");
+	}
+	else if(interSession)
+	{
+		printf("Inter-session only\n");
+	}
+
+	if(!intraSession && !interSession)
+	{
+		intraSession = true;
+		interSession = true;
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
+	{
+		printf(" Using parameter \"%s=%s\" from arguments\n", iter->first.c_str(), iter->second.c_str());
+	}
+
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	printf("Initialization...\n");
+	uInsert(parameters, inputParams);
+	rtabmap.init(parameters, dbPath);
+
+	float xMin, yMin, cellSize;
+	bool haveOptimizedMap = !rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize).empty();
+
+	PrintProgressState progress;
+	printf("Detecting...\n");
+	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin);
+	if(detected < 0)
+	{
+		if(!g_loopForever)
+		{
+			printf("Detection interrupted. Loop closures found so far (if any) are not saved.\n");
+		}
+		else
+		{
+			printf("Loop closure detection failed!\n");
+		}
+	}
+	else if(detected > 0 && haveOptimizedMap)
+	{
+		printf("The database has a global occupancy grid, regenerating one with new optimized graph!\n");
+		LocalGridCache cache;
+		OccupancyGrid grid(&cache, parameters);
+		std::map<int, Transform> optimizedPoses = rtabmap.getLocalOptimizedPoses();
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter)
+		{
+			SensorData data = rtabmap.getMemory()->getNodeData(iter->first, false, false, false, true);
+			data.uncompressData();
+			cache.add(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
+		}
+		grid.update(optimizedPoses);
+		cv::Mat map = grid.getMap(xMin, yMin);
+		if(map.empty())
+		{
+			printf("Could not generate the global occupancy grid!\n");
+		}
+		else
+		{
+			rtabmap.getMemory()->save2DMap(map, xMin, yMin, cellSize);
+			printf("Save new global occupancy grid!\n");
+		}
+	}
+
+	rtabmap.close();
+
+	return 0;
+}

rtabmap/tools/EpipolarGeometry/CMakeLists.txt

@@ -0,0 +1,11 @@
+
+IF(MINGW)
+  ADD_EXECUTABLE(epipolar_geometry WIN32 main.cpp)
+ELSE()
+  ADD_EXECUTABLE(epipolar_geometry main.cpp)
+ENDIF()
+TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_gui)
+
+SET_TARGET_PROPERTIES( epipolar_geometry 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-epipolar_geometry)
+

rtabmap/tools/EpipolarGeometry/main.cpp

@@ -0,0 +1,363 @@
+/*
+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 <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <iostream>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/EpipolarGeometry.h"
+#include "rtabmap/core/VWDictionary.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/utilite/UCv2Qt.h"
+
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/gui/KeypointItem.h"
+#include <QApplication>
+#include <QGraphicsLineItem>
+#include <QGraphicsPixmapItem>
+#include <QVBoxLayout>
+#include <QHBoxLayout>
+#include <QtCore/QTime>
+#include <QtCore/QElapsedTimer>
+#include <QGraphicsEffect>
+
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-epipolar_geometry image1.jpg image2.jpg\n");
+	exit(1);
+}
+
+class MainWidget : public QWidget
+{
+public:
+	MainWidget(const cv::Mat & image1,
+			   const cv::Mat & image2,
+			   const std::multimap<int, cv::KeyPoint> & words1,
+			   const std::multimap<int, cv::KeyPoint> & words2,
+			   const std::vector<uchar> & status)
+	{
+		view1_ = new ImageView(this);
+		this->setLayout(new QHBoxLayout());
+		this->layout()->setSpacing(0);
+		this->layout()->setContentsMargins(0,0,0,0);
+		this->layout()->addWidget(view1_);
+		view1_->setSceneRect(QRectF(0,0,(float)image1.cols, (float)image1.rows));
+		view1_->setLinesShown(true);
+		view1_->setFeaturesShown(false);
+		view1_->setImageDepthShown(true);
+
+		view1_->setImage(uCvMat2QImage(image1));
+		view1_->setImageDepth(image2);
+
+		drawKeypoints(words1, words2, status);
+	}
+protected:
+	virtual void showEvent(QShowEvent* event)
+	{
+		resizeEvent(0);
+	}
+private:
+	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords, const std::vector<uchar> & status)
+	{
+		UTimer timer;
+
+		timer.start();
+		QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
+		QList<bool> inliers;
+		int j=0;
+		for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
+		{
+			int id = (*i).first;
+			QColor color;
+			if(uContains(loopWords, id))
+			{
+				// PINK = FOUND IN LOOP SIGNATURE
+				color = Qt::magenta;
+				//To draw lines... get only unique correspondences
+				if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
+				{
+					uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(i->second.pt, uValues(loopWords, id).begin()->pt));
+					inliers.push_back(status[j++]);
+				}
+			}
+			else if(refWords.count(id) > 1)
+			{
+				// YELLOW = NEW and multiple times
+				color = Qt::yellow;
+			}
+			else
+			{
+				// GREEN = NEW
+				color = Qt::green;
+			}
+			view1_->addFeature(id, i->second, 0, color);
+		}
+		ULOGGER_DEBUG("source time = %f s", timer.ticks());
+
+		// Draw lines between corresponding features...
+		UASSERT(uniqueCorrespondences.size() == inliers.size());
+		QList<bool>::iterator jter = inliers.begin();
+		for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
+			iter!=uniqueCorrespondences.end();
+			++iter)
+		{
+			view1_->addLine(
+					iter->first.x,
+					iter->first.y,
+					iter->second.x,
+					iter->second.y,
+					*jter?Qt::cyan:Qt::red);
+			++jter;
+		}
+		view1_->update();
+	}
+
+private:
+	ImageView * view1_;
+};
+
+std::multimap<int, cv::KeyPoint> aggregate(const std::list<int> & wordIds, const std::vector<cv::KeyPoint> & keypoints)
+{
+	std::multimap<int, cv::KeyPoint> words;
+	std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
+	for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
+	{
+		words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
+		++kpIter;
+	}
+	return words;
+}
+
+
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	cv::Mat image1;
+    cv::Mat image2;
+	if(argc == 3)
+	{
+		image1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
+		image2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
+	}
+	else
+	{
+		showUsage();
+	}
+
+	QElapsedTimer timer;
+	timer.start();
+
+	// Extract words
+   VWDictionary dictionary;
+   ParametersMap param;
+   param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
+   param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
+   SURF detector(param);
+   std::vector<cv::KeyPoint> kpts1 = detector.generateKeypoints(image1);
+   std::vector<cv::KeyPoint> kpts2 = detector.generateKeypoints(image2);
+   cv::Mat descriptors1 = detector.generateDescriptors(image1, kpts1);
+   cv::Mat descriptors2 = detector.generateDescriptors(image2, kpts2);
+   UINFO("detect/extract features = %d ms", timer.elapsed());
+
+   timer.start();
+   std::list<int> wordIds1 = dictionary.addNewWords(descriptors1, 1);
+   dictionary.update();
+   std::list<int> wordIds2 = dictionary.addNewWords(descriptors2, 2);
+   UINFO("quantization to words = %d ms", timer.elapsed());
+
+   std::multimap<int, cv::KeyPoint> words1 = aggregate(wordIds1, kpts1);
+   std::multimap<int, cv::KeyPoint> words2 = aggregate(wordIds2, kpts2);
+
+   // Find pairs
+   timer.start();
+   std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+   EpipolarGeometry::findPairsUnique(words1, words2, pairs);
+   UINFO("find pairs = %d ms", timer.elapsed());
+
+   // Find fundamental matrix
+   timer.start();
+   std::vector<uchar> status;
+   cv::Mat fundamentalMatrix = EpipolarGeometry::findFFromWords(pairs, status);
+   UINFO("inliers = %d/%d", uSum(status), pairs.size());
+   UINFO("find F = %d ms", timer.elapsed());
+   if(!fundamentalMatrix.empty())
+   {
+	   int i = 0;
+	   int goodCount = 0;
+	   for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
+		{
+			if(status[i])
+			{
+				// the output of the correspondences can be easily copied in MatLab
+				if(goodCount==0)
+				{
+					printf("x=[%f %f %d]; xp=[%f %f %d];\n",
+							iter->second.first.pt.x,
+							iter->second.first.pt.y,
+							iter->first,
+							iter->second.second.pt.x,
+							iter->second.second.pt.y,
+							iter->first);
+				}
+				else
+				{
+					printf("x=[x;[%f %f %d]]; xp=[xp;[%f %f %d]];\n",
+							iter->second.first.pt.x,
+							iter->second.first.pt.y,
+							iter->first,
+							iter->second.second.pt.x,
+							iter->second.second.pt.y,
+							iter->first);
+				}
+				++goodCount;
+			}
+			++i;
+		}
+
+		// Show the fundamental matrix
+		std::cout << "F=" << fundamentalMatrix << std::endl;
+
+		// Intrinsic parameters K of the camera (guest... non-calibrated camera)
+		cv::Mat k = cv::Mat::zeros(3,3,CV_64FC1);
+		k.at<double>(0,0) = image1.cols; // focal x
+		k.at<double>(1,1) = image1.rows; // focal y
+		k.at<double>(2,2) = 1;
+		k.at<double>(0,2) = image1.cols/2; // center x in pixels
+		k.at<double>(1,2) = image1.rows/2; // center y in pixels
+
+		// Use essential matrix E=K'*F*K
+
+		cv::Mat e = k.t()*fundamentalMatrix*k;
+
+		//remove K from points xe = inv(K)*x
+		cv::Mat x1(2, goodCount, CV_64FC1);
+		cv::Mat x2(2, goodCount, CV_64FC1);
+		i=0;
+		int j=0;
+		cv::Mat invK = k.inv();
+		for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
+		{
+			if(status[i])
+			{
+				cv::Mat tmp(3,1,CV_64FC1);
+				tmp.at<double>(0,0) = iter->second.first.pt.x;
+				tmp.at<double>(1,0) = iter->second.first.pt.y;
+				tmp.at<double>(2,0) = 1;
+				tmp = invK*tmp;
+				x1.at<double>(0,j) = tmp.at<double>(0,0);
+				x1.at<double>(1,j) = tmp.at<double>(1,0);
+				tmp.at<double>(0,0) = iter->second.second.pt.x;
+				tmp.at<double>(1,0) = iter->second.second.pt.y;
+				tmp.at<double>(2,0) = 1;
+				tmp = invK*tmp;
+				x2.at<double>(0,j) = tmp.at<double>(0,0);
+				x2.at<double>(1,j) = tmp.at<double>(1,0);
+				UDEBUG("i=%d j=%d, x1=[%f,%f] x2=[%f,%f]", i, j, x1.at<double>(0,j), x1.at<double>(1,j), x2.at<double>(0,j), x2.at<double>(1,j));
+				++j;
+			}
+			++i;
+		}
+
+		std::cout<<"K=" << k << std::endl;
+		timer.start();
+		//std::cout<<"e=" << e << std::endl;
+		cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
+		cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
+		p0.at<double>(0,0) = 1;
+		p0.at<double>(1,1) = 1;
+		p0.at<double>(2,2) = 1;
+		UINFO("find P from F = %d ms", timer.elapsed());
+
+		std::cout<<"P=" << p << std::endl;
+
+		//find 4D homogeneous points
+		cv::Mat x4d;
+		timer.start();
+		cv::triangulatePoints(p0, p, x1, x2, x4d);
+		UINFO("find X (triangulate) = %d ms", timer.elapsed());
+
+		//Show 4D points
+		for(int i=0; i<x4d.cols; ++i)
+		{
+			x4d.at<double>(0,i) = x4d.at<double>(0,i)/x4d.at<double>(3,i);
+			x4d.at<double>(1,i) = x4d.at<double>(1,i)/x4d.at<double>(3,i);
+			x4d.at<double>(2,i) = x4d.at<double>(2,i)/x4d.at<double>(3,i);
+			x4d.at<double>(3,i) = x4d.at<double>(3,i)/x4d.at<double>(3,i);
+			if(i==0)
+			{
+				printf("X=[%f;%f;%f;%f];\n",
+					x4d.at<double>(0,i),
+					x4d.at<double>(1,i),
+					x4d.at<double>(2,i),
+					x4d.at<double>(3,i));
+			}
+			else
+			{
+				printf("X=[X [%f;%f;%f;%f]];\n",
+					x4d.at<double>(0,i),
+					x4d.at<double>(1,i),
+					x4d.at<double>(2,i),
+					x4d.at<double>(3,i));
+			}
+		}
+
+		//Show rotation/translation of the second camera
+		cv::Mat r;
+		cv::Mat t;
+		EpipolarGeometry::findRTFromP(p, r, t);
+		std::cout<< "R=" << r << std::endl;
+		std::cout<< "t=" << t << std::endl;
+
+		//GUI
+		QApplication app(argc, argv);
+		MainWidget mainWidget(image1, image2, words1, words2, status);
+		mainWidget.show();
+		app.exec();
+	}
+	else
+	{
+		UINFO("Fundamental matrix not found...");
+	}
+
+    return 0;
+}

rtabmap/tools/EurocDataset/CMakeLists.txt

@@ -0,0 +1,49 @@
+
+FIND_PACKAGE(yaml-cpp QUIET)
+
+IF(yaml-cpp_FOUND)
+  IF (TARGET yaml-cpp::yaml-cpp)
+    # yaml-cpp 0.8.0 uses target yaml-cpp::yaml-cpp.
+    SET(YAML_CPP_LIBRARIES yaml-cpp::yaml-cpp)
+  ELSEIF (TARGET yaml-cpp)
+    # yaml-cpp 0.7.0 uses target yaml-cpp (VCPKG).
+    SET(YAML_CPP_LIBRARIES yaml-cpp)
+  ENDIF()
+ELSE()
+   find_package(PkgConfig QUIET)
+   IF(PKG_CONFIG_FOUND)
+      pkg_check_modules(yaml_cpp QUIET yaml-cpp)
+	  IF(yaml_cpp_FOUND)
+         SET(YAML_CPP_LIBRARIES ${yaml_cpp_LIBRARIES})
+         SET(YAML_CPP_INCLUDE_DIR ${yaml_cpp_INCLUDEDIR})
+         SET(yaml-cpp_FOUND ${yaml_cpp_FOUND})
+	  ENDIF(yaml_cpp_FOUND)
+   ENDIF(PKG_CONFIG_FOUND)
+ENDIF(yaml-cpp_FOUND)
+
+IF(yaml-cpp_FOUND)
+
+SET(INCLUDE_DIRS
+    ${YAML_CPP_INCLUDE_DIR}
+)
+
+SET(LIBRARIES
+	${YAML_CPP_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS} yaml-cpp)
+
+ADD_EXECUTABLE(euroc_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(euroc_dataset rtabmap_core ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( euroc_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset)
+    
+INSTALL(TARGETS euroc_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+	
+ELSE()
+  MESSAGE(STATUS "yaml-cpp not found, euroc_dataset tool won't be built...")
+ENDIF()

rtabmap/tools/EurocDataset/main.cpp

@@ -0,0 +1,701 @@
+/*
+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/CameraStereo.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include "rtabmap/core/IMUFilter.h"
+#include <pcl/common/common.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <yaml-cpp/yaml.h>
+#include <stdio.h>
+#include <signal.h>
+#include <fstream>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-kitti_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/EuRoC/V1_03_difficult\")\n"
+			"                        containing least mav0/cam0/sensor.yaml, mav0/cam1/sensor.yaml and \n"
+			"                        mav0/cam0/data and mav0/cam1/data folders.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"  --exposure_comp    Do exposure compensation between left and right images.\n"
+			"  --disp             Generate full disparity.\n"
+			"  --raw              Use raw images (not rectified, this only works with okvis, msckf or vins odometry).\n"
+			"  --imu #            IMU filter: 0=madgwick, 1=complementary (default).\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-euroc_dataset \\\n"
+			"       --Rtabmap/PublishRAMUsage true\\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
+			"       --RGBD/LinearUpdate 0\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       ~/EuRoC/V1_03_difficult\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	std::string seq;
+	bool disp = false;
+	bool raw = false;
+	bool exposureCompensation = false;
+	bool quiet = false;
+	int imuFilter = 1;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+			else if(std::strcmp(argv[i], "--disp") == 0)
+			{
+				disp = true;
+			}
+			else if(std::strcmp(argv[i], "--raw") == 0)
+			{
+				raw = true;
+			}
+			else if(std::strcmp(argv[i], "--imu") == 0)
+			{
+				imuFilter = atoi(argv[++i]);
+			}
+			else if(std::strcmp(argv[i], "--exposure_comp") == 0)
+			{
+				exposureCompensation = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+		if(raw)
+		{
+			parameters.insert(ParametersPair(Parameters::kRtabmapImagesAlreadyRectified(), "false"));
+		}
+	}
+
+	seq = uSplit(path, '/').back();
+	std::string pathLeftImages  = path+"/mav0/cam0/data";
+	std::string pathRightImages = path+"/mav0/cam1/data";
+	std::string pathCalibLeft = path+"/mav0/cam0/sensor.yaml";
+	std::string pathCalibRight = path+"/mav0/cam1/sensor.yaml";
+	std::string pathGt = path+"/mav0/state_groundtruth_estimate0/data.csv";
+	std::string pathImu = path+"/mav0/imu0/data.csv";
+	if(!UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+
+	printf("Paths:\n"
+			"   Sequence number:  %s\n"
+			"   Sequence path:    %s\n"
+			"   Output:           %s\n"
+			"   Output name:      %s\n"
+			"   left images:      %s\n"
+			"   right images:     %s\n"
+			"   left calib:       %s\n"
+			"   right calib:      %s\n",
+			seq.c_str(),
+			path.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			pathLeftImages.c_str(),
+			pathRightImages.c_str(),
+			pathCalibLeft.c_str(),
+			pathCalibRight.c_str());
+	if(!pathGt.empty())
+	{
+		printf("   Ground truth:     %s\n", pathGt.c_str());
+	}
+	if(!pathImu.empty())
+	{
+		printf("   IMU:              %s\n", pathImu.c_str());
+		printf("   IMU Filter:       %d\n", imuFilter);
+	}
+
+	printf("   Exposure Compensation: %s\n", exposureCompensation?"true":"false");
+	printf("   Disparity:        %s\n", disp?"true":"false");
+	printf("   Raw images:       %s\n", raw?"true (Rtabmap/ImagesAlreadyRectified set to false)":"false");
+
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	std::vector<CameraModel> models;
+	int rateHz = 20;
+	for(int k=0; k<2; ++k)
+	{
+		// Left calibration
+		std::string calibPath = k==0?pathCalibLeft:pathCalibRight;
+		YAML::Node config = YAML::LoadFile(calibPath);
+		if(config.IsNull())
+		{
+			UERROR("Cannot open calibration file \"%s\"", calibPath.c_str());
+			return -1;
+		}
+
+		YAML::Node T_BS = config["T_BS"];
+		YAML::Node data = T_BS["data"];
+		UASSERT(data.size() == 16);
+		rateHz = config["rate_hz"].as<int>();
+		YAML::Node resolution = config["resolution"];
+		UASSERT(resolution.size() == 2);
+		YAML::Node intrinsics = config["intrinsics"];
+		UASSERT(intrinsics.size() == 4);
+		YAML::Node distortion_coefficients = config["distortion_coefficients"];
+		UASSERT(distortion_coefficients.size() == 4 || distortion_coefficients.size() == 5 || distortion_coefficients.size() == 8);
+
+		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+		K.at<double>(0,0) = intrinsics[0].as<double>();
+		K.at<double>(1,1) = intrinsics[1].as<double>();
+		K.at<double>(0,2) = intrinsics[2].as<double>();
+		K.at<double>(1,2) = intrinsics[3].as<double>();
+		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+		cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+		K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+		cv::Mat D = cv::Mat::zeros(1, distortion_coefficients.size(), CV_64FC1);
+		for(unsigned int i=0; i<distortion_coefficients.size(); ++i)
+		{
+			D.at<double>(i) = distortion_coefficients[i].as<double>();
+		}
+
+		Transform t(data[0].as<float>(), data[1].as<float>(), data[2].as<float>(), data[3].as<float>(),
+					data[4].as<float>(), data[5].as<float>(), data[6].as<float>(), data[7].as<float>(),
+					data[8].as<float>(), data[9].as<float>(), data[10].as<float>(), data[11].as<float>());
+
+		models.push_back(CameraModel(outputName+"_calib", cv::Size(resolution[0].as<int>(),resolution[1].as<int>()), K, D, R, P, t));
+		UASSERT(models.back().isValidForRectification());
+	}
+
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+
+	StereoCameraModel model(outputName+"_calib", models[0], models[1], models[1].localTransform().inverse() * models[0].localTransform());
+	if(!model.save(output, false))
+	{
+		UERROR("Could not save calibration!");
+		return -1;
+	}
+	printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
+
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	// We use CameraThread only to use postUpdate() method
+	Transform baseToImu(0,0,1,0, 0,-1,0,0, 1,0,0,0);
+	SensorCaptureThread cameraThread(new
+		CameraStereoImages(
+				pathLeftImages,
+				pathRightImages,
+				!raw,
+				0.0f,
+				baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse()), parameters);
+	printf("baseToImu=%s\n", baseToImu.prettyPrint().c_str());
+	std::cout<<"baseToCam0:\n" << baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse() << std::endl;
+	printf("baseToCam0=%s\n", (baseToImu*models[0].localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint().c_str());
+	printf("imuToCam0=%s\n", models[0].localTransform().prettyPrint().c_str());
+	printf("imuToCam1=%s\n", models[1].localTransform().prettyPrint().c_str());
+	((CameraStereoImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(exposureCompensation)
+	{
+		cameraThread.setStereoExposureCompensation(true);
+	}
+	if(disp)
+	{
+		cameraThread.setStereoToDepth(true);
+	}
+	if(!pathGt.empty())
+	{
+		((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 9);
+	}
+
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+
+	int mapUpdate = rateHz / detectionRate;
+	if(mapUpdate < 1)
+	{
+		mapUpdate = 1;
+	}
+
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(output, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+
+		std::ifstream imu_file;
+
+		// open the IMU file
+		std::string line;
+		imu_file.open(pathImu.c_str());
+		if (!imu_file.good()) {
+			UERROR("no imu file found at %s",pathImu.c_str());
+			return -1;
+		}
+		int number_of_lines = 0;
+		while (std::getline(imu_file, line))
+			++number_of_lines;
+		printf("No. IMU measurements: %d\n", number_of_lines-1);
+		if (number_of_lines - 1 <= 0) {
+			UERROR("no imu messages present in %s", pathImu.c_str());
+			return -1;
+		}
+		// set reading position to second line
+		imu_file.clear();
+		imu_file.seekg(0, std::ios::beg);
+		std::getline(imu_file, line);
+
+		if(odomStrategy == Odometry::kTypeMSCKF)
+		{
+			if(seq.compare("MH_01_easy") == 0)
+			{
+				printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n");
+				((CameraStereoImages*)cameraThread.camera())->setStartIndex(440);
+			}
+			else if(seq.compare("MH_02_easy") == 0)
+			{
+				printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n");
+				((CameraStereoImages*)cameraThread.camera())->setStartIndex(525);
+			}
+			else if(seq.compare("MH_03_medium") == 0)
+			{
+				printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n");
+				((CameraStereoImages*)cameraThread.camera())->setStartIndex(210);
+			}
+			else if(seq.compare("MH_04_difficult") == 0)
+			{
+				printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n");
+				((CameraStereoImages*)cameraThread.camera())->setStartIndex(250);
+			}
+			else if(seq.compare("MH_05_difficult") == 0)
+			{
+				printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n");
+				((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
+			}
+		}
+
+		cameraThread.enableIMUFiltering(imuFilter, parameters);
+
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		SensorCaptureInfo cameraInfo;
+		UDEBUG("");
+		SensorData data = cameraThread.camera()->takeData(&cameraInfo);
+		UDEBUG("");
+		int iteration = 0;
+		double start = data.stamp();
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		cv::Mat covariance;
+		int odomKeyFrames = 0;
+		while(data.isValid() && g_forever)
+		{
+			UDEBUG("");
+			// get all IMU measurements till then
+			double t_imu = start;
+			do {
+				std::string line;
+				if (!std::getline(imu_file, line)) {
+					std::cout << std::endl << "Finished parsing IMU." << std::endl << std::flush;
+					break;
+				}
+
+				std::stringstream stream(line);
+				std::string s;
+				std::getline(stream, s, ',');
+				std::string nanoseconds = s.substr(s.size() - 9, 9);
+				std::string seconds = s.substr(0, s.size() - 9);
+
+				cv::Vec3d gyr;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ',');
+					gyr[j] = uStr2Double(s);
+				}
+
+				cv::Vec3d acc;
+				for (int j = 0; j < 3; ++j) {
+					std::getline(stream, s, ',');
+					acc[j] = uStr2Double(s);
+				}
+
+				t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
+
+				if (t_imu - start + 1 > 0) {
+
+					SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
+					cameraThread.postUpdate(&dataImu);
+					odom->process(dataImu);
+				}
+
+			} while (t_imu <= data.stamp());
+
+
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			OdometryInfo odomInfo;
+			UDEBUG("");
+			Transform pose = odom->process(data, &odomInfo);
+			UDEBUG("");
+
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			if(odomStrategy == Odometry::kTypeFovis)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+
+			bool processData = true;
+			if(iteration % mapUpdate != 0)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+			if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
+			{
+				covariance = odomInfo.reg.covariance;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
+				covariance = cv::Mat();
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(data.keypoints().size() == 0 && data.laserScanRaw().size())
+				{
+					if(rmse >= 0.0f)
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				else
+				{
+					if(rmse >= 0.0f)
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = SensorCaptureInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeData(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory ...\n");
+		std::map<int, Transform> poses;
+		std::map<int, Transform> vo_poses;
+		std::multimap<int, Link> links;
+		std::map<int, Signature> signatures;
+		std::map<int, double> stamps;
+		rtabmap.getGraph(vo_poses, links, false, true);
+		links.clear();
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			// vo performance
+			graph::calcRMSE(
+					groundTruth,
+					vo_poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+			float translational_rmse_vo = translational_rmse;
+			float rotational_rmse_vo = rotational_rmse;
+			// SLAM performance
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m   (vo = %f m)\n", translational_rmse, translational_rmse_vo);
+			printf("   rotational_rmse=      %f deg (vo = %f deg)\n", rotational_rmse, rotational_rmse_vo);
+
+			FILE * pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}

rtabmap/tools/Export/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(export main.cpp)
+  
+TARGET_LINK_LIBRARIES(export rtabmap_core)
+
+SET_TARGET_PROPERTIES( export 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-export)
+
+INSTALL(TARGETS export
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

rtabmap/tools/ExtractObject/CMakeLists.txt

@@ -0,0 +1,11 @@
+
+
+ADD_EXECUTABLE(extractObject main.cpp)
+TARGET_LINK_LIBRARIES(extractObject rtabmap_core)
+
+SET_TARGET_PROPERTIES( extractObject 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-extractObject)
+
+INSTALL(TARGETS extractObject
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/ExtractObject/main.cpp

@@ -0,0 +1,267 @@
+/*
+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 <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/common/common.h>
+#include <pcl/common/pca.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <rtabmap/utilite/UConversion.h>
+
+void showUsage()
+{
+	printf("\nUsage: extractObject [options] cloud.pcd\n"
+			"Options:\n"
+			"   -p #.#            plane distance threshold (default 0.02 m)\n"
+			"   -a #.#            plane angle tolerance from z axis (default PI/6)\n"
+			"   -c #.#            cluster tolerance (default 0.1 m)\n"
+			"   -s #              minimum cluster size (default 50 points)\n"
+			"   -center_obj       center the objects to their local reference\n"
+			"   -save_plane       save the plane inliers to \"extracted_plane.pcd\"\n");
+}
+
+int main(int argc, char *argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+		return -1;
+	}
+
+	std::string cloudPath = argv[argc-1];
+	double planeDistanceThreshold = 0.02f;
+	double planeEpsAngle = 3.1416/6.0;
+	double clusterTolerance = 0.1f;
+	int minClusterSize = 50;
+	bool centerObject = false;
+	bool savePlane = false;
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "-p") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				planeDistanceThreshold = uStr2Float(argv[i]);
+				if(planeDistanceThreshold < 0.0f)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-a") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				planeEpsAngle = uStr2Float(argv[i]);
+				if(planeEpsAngle < 0.0f)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-c") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				clusterTolerance = uStr2Float(argv[i]);
+				if(clusterTolerance <= 0.0f)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-s") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				minClusterSize = std::atoi(argv[i]);
+				if(minClusterSize < 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-center_obj") == 0)
+		{
+			centerObject = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-save_plane") == 0)
+		{
+			savePlane = true;
+			continue;
+		}
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	printf("Parameters:\n"
+			"   planeDistanceThreshold=%f\n"
+			"   planeEpsAngle=%f\n"
+			"   clusterTolerance=%f\n"
+			"   minClusterSize=%d\n"
+			"   centerObject=%s\n"
+			"   savePlane=%s\n",
+			planeDistanceThreshold,
+			planeEpsAngle,
+			clusterTolerance,
+			minClusterSize,
+			centerObject?"true":"false",
+			savePlane?"true":"false");
+
+	printf("Loading \"%s\"...", cloudPath.c_str());
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::io::loadPCDFile(cloudPath, *cloud);
+	printf("done! (%d points)\n", (int)cloud->size());
+
+	// Extract plane
+	pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+	pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+	// Create the segmentation object
+	pcl::SACSegmentation<pcl::PointXYZRGB> seg;
+	// Optional
+	seg.setOptimizeCoefficients (true);
+	// Mandatory
+	seg.setModelType (pcl::SACMODEL_PLANE);
+	seg.setMethodType (pcl::SAC_RANSAC);
+	seg.setMaxIterations(1000);
+	seg.setDistanceThreshold (planeDistanceThreshold);
+	seg.setAxis(Eigen::Vector3f(0,0,1));
+	seg.setEpsAngle (planeEpsAngle);
+
+	seg.setInputCloud (cloud);
+	seg.segment (*inliers, *coefficients);
+
+	printf("Plane coefficients: %f %f %f %f\n", coefficients->values[0],
+										  coefficients->values[1],
+										  coefficients->values[2],
+										  coefficients->values[3]);
+
+	float a = coefficients->values[0];
+	float b = coefficients->values[1];
+	float c = coefficients->values[2];
+	float d = coefficients->values[3];
+
+	// Create a quaternion for rotation into XY plane
+	Eigen::Vector3f current(a, b, c);
+	Eigen::Vector3f target(0.0, 0.0, 1.0);
+	Eigen::Quaternion<float> q;
+	q.setFromTwoVectors(current, target);
+	Eigen::Matrix4f trans;
+	trans.topLeftCorner<3,3>() = q.toRotationMatrix();
+
+	float planeShift = -d;
+
+	// Create transformed point cloud (polygon is aligned with XY plane)
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB> ());
+	pcl::transformPointCloud(*cloud, *output, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), q);
+
+	if(savePlane)
+	{
+		pcl::io::savePCDFile("extracted_plane.pcd", *output, inliers->indices);
+		printf("Saved extracted_plane.pcd (%d points)\n", (int)inliers->indices.size());
+	}
+	else
+	{
+		printf("Plane size = %d points\n", (int)inliers->indices.size());
+	}
+
+	// remove plane inliers
+	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
+	extract.setNegative (true);
+	extract.setInputCloud (output);
+	extract.setIndices(inliers);
+	extract.filter (*output);
+
+	// Get the biggest cluster
+	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZRGB>);
+	kdTree->setInputCloud(output);
+	std::vector<pcl::PointIndices> cluster_indices;
+	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
+	ec.setClusterTolerance (clusterTolerance);
+	ec.setMinClusterSize (minClusterSize);
+	ec.setMaxClusterSize (200000);
+	ec.setSearchMethod (kdTree);
+	ec.setInputCloud (output);
+	ec.extract (cluster_indices);
+
+	if(cluster_indices.size() == 0)
+	{
+		printf("No object found! (minimum cluster size=%d)\n", minClusterSize);
+		return 1;
+	}
+
+	for(unsigned int i=0; i<cluster_indices.size(); ++i)
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr object (new pcl::PointCloud<pcl::PointXYZRGB> ());
+		pcl::copyPointCloud(*output, cluster_indices.at(i), *object);
+
+		if(centerObject)
+		{
+			// recenter the object on xy plane, and set min z to 0
+			Eigen::Vector4f min, max;
+			pcl::getMinMax3D(*object, min, max);
+
+			pcl::transformPointCloud(*object, *object, Eigen::Vector3f(-(max[0]+min[0])/2.0f, -(max[1]+min[1])/2.0f, -min[2]), Eigen::Quaternion<float>(0,0,0,0));
+		}
+
+		pcl::io::savePCDFile(uFormat("extracted_object%d.pcd", (int)i+1), *object);
+		printf("Saved extracted_object%d.pcd (%d points)\n", (int)i+1, (int)object->size());
+	}
+
+	return 0;
+}

rtabmap/tools/GlobalBundleAdjustment/CMakeLists.txt

@@ -0,0 +1,13 @@
+
+ADD_EXECUTABLE(globalBundleAdjustment main.cpp)
+  
+TARGET_LINK_LIBRARIES(globalBundleAdjustment rtabmap_core)
+
+SET_TARGET_PROPERTIES( globalBundleAdjustment 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-globalBundleAdjustment)
+
+INSTALL(TARGETS globalBundleAdjustment
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

rtabmap/tools/GlobalBundleAdjustment/main.cpp

@@ -0,0 +1,138 @@
+/*
+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/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-globalBundleAdjustment database.db\n"
+			"\n%s", Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		if(uStrNumCmp(driver->getDatabaseVersion(), "0.17.0")<0)
+		{
+			printf("Database is too old (%s), we cannot save back optimized poses. "
+					"Consider upgrading the database with:\n"
+					"rtabmap-reprocess --Db/TargetVersion \"\" \"%s\" \"output.db\"\n",
+					driver->getDatabaseVersion().c_str(),
+					dbPath.c_str());
+			driver->closeConnection(false);
+			delete driver;
+			return -1;
+		}
+
+		parameters = driver->getLastParameters();
+		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+		driver->save2DMap(cv::Mat(), 0, 0, 0);
+		driver->saveOptimizedMesh(cv::Mat());
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
+	{
+		printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
+	}
+
+	UTimer timer;
+
+	printf("Loading database \"%s\"...\n", dbPath.c_str());
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	uInsert(parameters, inputParams);
+	rtabmap.init(parameters, dbPath);
+	printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
+
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	printf("Optimizing the map...\n");
+	rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+	printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
+
+	printf("Global bundle adjustment...\n");
+	Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
+	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true, parameters);
+	delete optimizer;
+	printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
+
+	if(!optimizedPoses.empty())
+	{
+		rtabmap.setOptimizedPoses(optimizedPoses, links);
+	}
+	else
+	{
+		UERROR("Returned empty poses!");
+	}
+
+	rtabmap.close();
+
+	return 0;
+}

rtabmap/tools/ImagesJoiner/CMakeLists.txt

@@ -0,0 +1,7 @@
+
+ADD_EXECUTABLE(imagesJoiner main.cpp)
+TARGET_LINK_LIBRARIES(imagesJoiner rtabmap_core)
+
+SET_TARGET_PROPERTIES( imagesJoiner 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesJoiner)
+

rtabmap/tools/ImagesJoiner/main.cpp

@@ -0,0 +1,171 @@
+/*
+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/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"imagesJoiner.exe [option] path\n"
+			"imagesJoiner.exe path_left path_right\n"
+			"  Options:\n"
+			"    -inv       option for copying odd images on the right\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	bool inv = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "-inv") == 0)
+		{
+			inv = true;
+			printf(" Inversing option activated...\n");
+			continue;
+		}
+		if(argc > 3)
+		{
+			showUsage();
+			printf(" Not recognized option: \"%s\"\n", argv[i]);
+		}
+	}
+
+	std::string path, pathRight;
+
+	if(argc == 3 && !inv)
+	{
+		//two paths
+		path = argv[1];
+		pathRight = argv[2];
+
+		printf(" Path left = %s\n", path.c_str());
+		printf(" Path right = %s\n", pathRight.c_str());
+	}
+	else
+	{
+		path = argv[argc-1];
+		printf(" Path = %s\n", path.c_str());
+	}
+
+	UDirectory dir(path, "jpg bmp png tiff jpeg");
+	UDirectory dirRight(pathRight, "jpg bmp png tiff jpeg");
+	if(!dir.isValid() || (!pathRight.empty() && !dirRight.isValid()))
+	{
+		printf("Path invalid!\n");
+		exit(-1);
+	}
+
+	std::string targetDirectory = path+"_joined";
+	UDirectory::makeDir(targetDirectory);
+	printf(" Creating directory \"%s\"\n", targetDirectory.c_str());
+
+
+	std::string fileNameA = dir.getNextFilePath();
+	std::string fileNameB;
+	if(dirRight.isValid())
+	{
+		fileNameB = dirRight.getNextFilePath();
+	}
+	else
+	{
+		fileNameB = dir.getNextFilePath();
+	}
+
+	int i=1;
+	while(!fileNameA.empty() && !fileNameB.empty())
+	{
+		if(inv)
+		{
+			std::string tmp = fileNameA;
+			fileNameA = fileNameB;
+			fileNameB = tmp;
+		}
+
+		std::string ext = UFile::getExtension(fileNameA);
+
+		std::string targetFilePath = targetDirectory+UDirectory::separator()+uNumber2Str(i++)+"."+ext;
+
+		cv::Mat imageA = cv::imread(fileNameA.c_str());
+		cv::Mat imageB = cv::imread(fileNameB.c_str());
+
+		fileNameA.clear();
+		fileNameB.clear();
+
+		if(!imageA.empty() && !imageB.empty())
+		{
+			cv::Size sizeA = imageA.size();
+			cv::Size sizeB = imageB.size();
+			cv::Size targetSize(0,0);
+			targetSize.width = sizeA.width + sizeB.width;
+			targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height;
+			cv::Mat targetImage(targetSize, imageA.type());
+
+			cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height ));
+			imageA.copyTo(roiA);
+			cv::Mat roiB( targetImage, cv::Rect( sizeA.width, 0, sizeB.width, sizeB.height ) );
+			imageB.copyTo(roiB);
+
+			if(!cv::imwrite(targetFilePath.c_str(), targetImage))
+			{
+				printf("Error : saving to \"%s\" goes wrong...\n", targetFilePath.c_str());
+			}
+			else
+			{
+				printf("Saved \"%s\" \n", targetFilePath.c_str());
+			}
+
+			fileNameA = dir.getNextFilePath();
+			if(dirRight.isValid())
+			{
+				fileNameB = dirRight.getNextFilePath();
+			}
+			else
+			{
+				fileNameB = dir.getNextFilePath();
+			}
+		}
+		else
+		{
+			printf("Error: loading images failed!\n");
+		}
+	}
+	printf("%d files processed\n", i-1);
+
+	return 0;
+}

rtabmap/tools/Info/CMakeLists.txt

@@ -0,0 +1,14 @@
+
+ADD_EXECUTABLE(info main.cpp)
+  
+TARGET_LINK_LIBRARIES(info rtabmap_core)
+
+SET_TARGET_PROPERTIES( info 
+	PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-info)
+
+INSTALL(TARGETS info
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
+

rtabmap/tools/Info/main.cpp

@@ -0,0 +1,462 @@
+/*
+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 <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <signal.h>
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/VisualWord.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UStl.h"
+
+using namespace rtabmap;
+
+#ifdef _WIN32
+#include <Windows.h>
+#define COLOR_NORMAL FOREGROUND_BLUE | FOREGROUND_GREEN | FOREGROUND_RED
+#define COLOR_RED FOREGROUND_RED | FOREGROUND_INTENSITY
+#define COLOR_GREEN FOREGROUND_GREEN
+#define COLOR_YELLOW FOREGROUND_GREEN | FOREGROUND_RED
+#else
+#define COLOR_NORMAL "\033[0m"
+#define COLOR_RED "\033[31m"
+#define COLOR_GREEN "\033[32m"
+#define COLOR_YELLOW "\033[33m"
+#endif
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-info [options] \"map.db\"\n"
+			"  Options:\n"
+			"     --diff                   Show only modified parameters.\n"
+			"     --diff  \"other_map.db\"   Compare parameters with other database.\n"
+			"     --dump  \"config.ini\"     Dump parameters in ini file.\n"
+			"\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	std::string otherDatabasePath;
+	std::string dumpFilePath;
+	bool diff = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "--diff") == 0)
+		{
+			++i;
+			if(i<argc-1 && argv[i][0] != '-')
+			{
+				otherDatabasePath = uReplaceChar(argv[i], '~', UDirectory::homeDir());
+				printf("Comparing with other database \"%s\"...\n", otherDatabasePath.c_str());
+			}
+			diff = true;
+		}
+		if(strcmp(argv[i], "--dump") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				dumpFilePath = uReplaceChar(argv[i], '~', UDirectory::homeDir());
+			}
+			else
+			{
+				printf("--dump should have an output file path\n");
+				showUsage();
+			}
+		}
+	}
+
+	std::string databasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
+	if(!UFile::exists(databasePath))
+	{
+		printf("Database \"%s\" doesn't exist!\n", databasePath.c_str());
+		return -1;
+	}
+
+	DBDriver * driver = DBDriver::create();
+	if(!driver->openConnection(databasePath))
+	{
+		printf("Cannot open database \"%s\".\n", databasePath.c_str());
+		delete driver;
+		return -1;
+	}
+
+	ParametersMap parameters = driver->getLastParameters();
+	if(!dumpFilePath.empty())
+	{
+		Parameters::writeINI(dumpFilePath, parameters);
+		printf("%ld parameters exported to \"%s\".\n", parameters.size(), dumpFilePath.c_str());
+		return 0;
+	}
+	ParametersMap defaultParameters = Parameters::getDefaultParameters();
+	ParametersMap removedParameters = Parameters::getBackwardCompatibilityMap();
+	std::string otherDatabasePathName;
+	if(!otherDatabasePath.empty())
+	{
+		driver->closeConnection(false);
+
+		if(!UFile::exists(otherDatabasePath))
+		{
+			printf("Database \"%s\" doesn't exist!\n", otherDatabasePath.c_str());
+			delete driver;
+			return -1;
+		}
+
+		if(!driver->openConnection(otherDatabasePath))
+		{
+			printf("Cannot open database \"%s\".\n", otherDatabasePath.c_str());
+			delete driver;
+			return -1;
+		}
+		otherDatabasePathName = UFile::getName(otherDatabasePath);
+		defaultParameters = driver->getLastParameters();
+		removedParameters.clear();
+	}
+
+#ifdef _WIN32
+	HANDLE H = GetStdHandle(STD_OUTPUT_HANDLE);
+#endif
+	int padding = 35;
+	std::cout << ("Parameters (Yellow=modified, Red=old parameter not used anymore, NA=not in database):\n");
+	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		ParametersMap::const_iterator jter = defaultParameters.find(iter->first);
+		std::string defaultValue;
+		bool defaultValueSet = false;
+		if(jter == defaultParameters.end())
+		{
+			jter = removedParameters.find(iter->first);
+			if(jter != removedParameters.end())
+			{
+				defaultValue = jter->second;
+				defaultValueSet = true;
+			}
+		}
+		else
+		{
+			defaultValue = jter->second;
+			defaultValueSet = true;
+		}
+
+		if(defaultValueSet &&
+		   iter->second.compare(defaultValue) != 0 &&
+		   iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
+		{
+			bool different = true;
+			if(Parameters::getType(iter->first).compare("double") ==0 ||
+			   Parameters::getType(iter->first).compare("float") == 0)
+			{
+				if(uStr2Double(iter->second) == uStr2Double(defaultValue))
+				{
+					different = false;
+				}
+			}
+
+			if(different)
+			{
+				//yellow
+#ifdef _WIN32
+				SetConsoleTextAttribute(H,COLOR_YELLOW);
+#else
+				printf("%s", COLOR_YELLOW);
+#endif
+				std::cout << (uFormat("%s%s  (%s=%s)\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), defaultValue.c_str()));
+			}
+			else if(!diff)
+			{
+				//green
+#ifdef _WIN32
+				SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+				printf("%s", COLOR_NORMAL);
+#endif
+				std::cout << (uFormat("%s%s\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+			}
+		}
+		else if(!defaultValueSet)
+		{
+			//red
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_RED);
+#else
+			printf("%s", COLOR_RED);
+#endif
+			std::cout << (uFormat("%s%s  (%s=NA)\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str()));
+		}
+		else if(!diff)
+		{
+			//green
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+			printf("%s", COLOR_NORMAL);
+#endif
+			std::cout << (uFormat("%s%s\n", uPad(iter->first + "=", padding).c_str(), iter->second.c_str()));
+		}
+#ifdef _WIN32
+		SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+		printf("%s", COLOR_NORMAL);
+#endif
+	}
+
+	for(ParametersMap::iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
+	{
+		ParametersMap::const_iterator jter = parameters.find(iter->first);
+		if(jter == parameters.end())
+		{
+			//red
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_RED);
+#else
+			printf("%s", COLOR_RED);
+#endif
+			std::cout << (uFormat("%sNA  (%s=\"%s\")\n", uPad(iter->first + "=", padding).c_str(), otherDatabasePath.empty()?"default":otherDatabasePathName.c_str(), iter->second.c_str()));
+		
+#ifdef _WIN32
+			SetConsoleTextAttribute(H,COLOR_NORMAL);
+#else
+			printf("%s", COLOR_NORMAL);
+#endif
+		}
+	}
+
+	if(otherDatabasePath.empty())
+	{
+		printf("\nInfo:\n\n");
+		std::string info;
+		std::set<int> ids;
+		driver->getAllNodeIds(ids);
+		Transform lastLocalization;
+		std::map<int, Transform> optimizedPoses = driver->loadOptimizedPoses(&lastLocalization);
+		cv::Vec3f min, max;
+		if(!optimizedPoses.empty())
+		{
+			graph::computeMinMax(optimizedPoses, min, max);
+		}
+		std::multimap<int, int> mapIdsLinkedToLastGraph;
+		int lastMapId=0;
+		double previousStamp = 0.0f;
+		Transform previousPose;
+		float infoTotalOdom = 0.0f;
+		double infoTotalTime = 0.0f;
+		int sessions = !ids.empty()?1:0;
+		int odomPoses = 0;
+		int gtPoses = 0;
+		int gpsValues = 0;
+		for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			Transform p, g;
+			int w;
+			std::string l;
+			double s;
+			int mapId;
+			std::vector<float> v;
+			GPS gps;
+			EnvSensors sensors;
+			int id = *iter;
+			driver->getNodeInfo(id, p, mapId, w, l, s, g, v, gps, sensors);
+			if(!p.isNull())
+			{
+				++odomPoses;
+			}
+			if(!g.isNull())
+			{
+				++gtPoses;
+			}
+			if(gps.stamp()>0.0)
+			{
+				++gpsValues;
+			}
+			if(optimizedPoses.find(id) != optimizedPoses.end())
+			{
+				mapIdsLinkedToLastGraph.insert(std::make_pair(mapId, id));
+			}
+			if(iter!=ids.begin())
+			{
+				if(lastMapId == mapId)
+				{
+					if(!p.isNull() && !previousPose.isNull())
+					{
+						infoTotalOdom += p.getDistance(previousPose);
+					}
+
+					if(previousStamp > 0.0 && s > 0.0)
+					{
+						infoTotalTime += s-previousStamp;
+					}
+				}
+				else
+				{
+					++sessions;
+				}
+			}
+			lastMapId = mapId;
+			previousStamp=s;
+			previousPose=p;
+		}
+		std::cout << (uFormat("%s%s\n", uPad("Path:").c_str(), driver->getUrl().c_str()));
+		std::cout << (uFormat("%s%s\n", uPad("Version:").c_str(), driver->getDatabaseVersion().c_str()));
+		std::cout << (uFormat("%s%d\n", uPad("Sessions:").c_str(), sessions));
+		std::multimap<int, Link> links;
+		driver->getAllLinks(links, true, true);
+		bool reducedGraph = false;
+		std::vector<int> linkTypes(Link::kEnd, 0);
+		std::vector<std::vector<float> > linkLengths(Link::kEnd);
+		std::multimap<int, Link> uniqueLinks = graph::filterDuplicateLinks(links);
+		for(std::multimap<int, Link>::iterator iter=uniqueLinks.begin(); iter!=uniqueLinks.end(); ++iter)
+		{
+			if(iter->second.type() == Link::kNeighborMerged)
+			{
+				reducedGraph = true;
+			}
+			if(iter->second.type()>=0 && iter->second.type()<Link::kEnd)
+			{
+				++linkTypes[iter->second.type()];
+				linkLengths[iter->second.type()].push_back(iter->second.transform().getNorm());
+			}
+		}
+		if(reducedGraph)
+		{
+			std::cout << (uFormat("%s%f m (approx. as graph has been reduced)\n", uPad("Total odom:").c_str(), infoTotalOdom));
+		}
+		else
+		{
+			std::cout << (uFormat("%s%f m\n", uPad("Total odometry length:").c_str(), infoTotalOdom));
+		}
+
+		std::stringstream sessionsInOptGraphStr;
+		std::list<int> mapsLinkedToLastGraph = uUniqueKeys(mapIdsLinkedToLastGraph);
+		for(std::list<int>::iterator iter=mapsLinkedToLastGraph.begin(); iter!=mapsLinkedToLastGraph.end(); ++iter)
+		{
+			if(iter!=mapsLinkedToLastGraph.begin())
+			{
+				sessionsInOptGraphStr << ", ";
+			}
+			sessionsInOptGraphStr << *iter << "(" << mapIdsLinkedToLastGraph.count(*iter) << ")";
+		}
+
+		int lastWordIdId = 0;
+		int wordsDim = 0;
+		int wordsType = 0;
+		driver->getLastWordId(lastWordIdId);
+		if(lastWordIdId>0)
+		{
+			std::set<int> ids;
+			ids.insert(lastWordIdId);
+			std::list<VisualWord *> vws;
+			driver->loadWords(ids, vws);
+			if(!vws.empty())
+			{
+				wordsDim = vws.front()->getDescriptor().cols;
+				wordsType = vws.front()->getDescriptor().type();
+				delete vws.front();
+				vws.clear();
+			}
+		}
+
+		std::cout << (uFormat("%s%fs\n", uPad("Total time:").c_str(), infoTotalTime));
+		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", uPad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
+		std::cout << (uFormat("%s%d nodes and %d words\n", uPad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
+		std::cout << (uFormat("%s%d poses and %d links\n", uPad("Global graph:").c_str(), odomPoses, links.size()));
+		std::cout << (uFormat("%s%d poses (x=%d->%d, y=%d->%d, z=%d->%d)\n", uPad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size(), (int)min[0], (int)max[0], (int)min[1], (int)max[1], min[2], (int)max[2]));
+		std::cout << (uFormat("%s%d/%d [%s]\n", uPad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
+		std::cout << (uFormat("%s%d poses\n", uPad("Ground truth:").c_str(), gtPoses));
+		std::cout << (uFormat("%s%d poses\n", uPad("GPS:").c_str(), gpsValues));
+		std::cout << (uFormat("Links:\n"));
+		for(size_t i=0; i<linkTypes.size(); ++i)
+		{
+			float avg = uMean(linkLengths[i]);
+			float std = uVariance(linkLengths[i], avg);
+			float max = uMax(linkLengths[i]);
+			if(std>0)
+			{
+				std = std::sqrt(std);
+			}
+			std::cout << (uFormat("%s%d\t(length avg: %.2fm, std: %.2fm, max: %.2fm)\n", 
+				uPad(uFormat("  %s:", Link::typeName((Link::Type)i).c_str())).c_str(),
+				linkTypes[i],
+				avg,
+				std,
+				max));
+		}
+		std::cout << ("\n");
+		long total = 0;
+		long dbSize = UFile::length(driver->getUrl());
+		long mem = dbSize;
+		std::cout << (uFormat("%s%d %s\n", uPad("Database size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes"));
+		mem = driver->getNodesMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Nodes size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getLinksMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Links size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getImagesMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("RGB Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getDepthImagesMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Depth Images size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getCalibrationsMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Calibrations size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getGridsMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Grids size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getLaserScansMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Scans size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getUserDataMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("User data size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getWordsMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Dictionary size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getFeaturesMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Features size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = driver->getStatisticsMemoryUsed();
+		total+=mem;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Statistics size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		mem = dbSize - total;
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", uPad("Other (indexing, unused):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << ("\n");
+	}
+
+	return 0;
+}

rtabmap/tools/KittiDataset/CMakeLists.txt

@@ -0,0 +1,11 @@
+
+ADD_EXECUTABLE(kitti_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(kitti_dataset rtabmap_core)
+
+SET_TARGET_PROPERTIES( kitti_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset)
+    
+INSTALL(TARGETS kitti_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/KittiDataset/main.cpp

@@ -0,0 +1,693 @@
+/*
+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/CameraStereo.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include <pcl/common/common.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-kitti_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/KITTI/dataset/sequences/07\")\n"
+			"                        containing least calib.txt, times.txt, image_0 and image_1 folders.\n"
+			"                        Optional image_2, image_3 and velodyne folders.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --gt \"path\"        Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"  --color            Use color images for stereo (image_2 and image_3 folders).\n"
+			"  --height           Add car's height to camera local transform (1.67m).\n"
+			"  --disp             Generate full disparity.\n"
+			"  --exposure_comp    Do exposure compensation between left and right images.\n"
+			"  --scan             Include velodyne scan in node's data (use --scan_only to ignore image data).\n"
+			"  --scan_step #      Scan downsample step (default=1).\n"
+			"  --scan_voxel #.#   Scan voxel size (default 0.5 m).\n"
+			"  --scan_k           Scan normal K (default 0).\n"
+			"  --scan_radius      Scan normal radius (default 0).\n\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-kitti_dataset \\\n"
+			"       --Rtabmap/PublishRAMUsage true\\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
+			"       --Rtabmap/CreateIntermediateNodes true\\\n"
+			"       --RGBD/LinearUpdate 0\\\n"
+			"       --GFTT/QualityLevel 0.01\\\n"
+			"       --GFTT/MinDistance 7\\\n"
+			"       --OdomF2M/MaxSize 3000\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       --Kp/MaxFeatures 750\\\n"
+			"       --Vis/MaxFeatures 1500\\\n"
+			"       --gt \"~/KITTI/devkit/cpp/data/odometry/poses/07.txt\"\\\n"
+			"       ~/KITTI/dataset/sequences/07\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	std::string seq;
+	bool color = false;
+	bool height = false;
+	bool scan = false;
+	bool disp = false;
+	bool exposureCompensation = false;
+	int scanStep = 1;
+	float scanVoxel = 0.5f;
+	int scanNormalK = 0;
+	float scanNormalRadius = 0.0f;
+	bool scanOnly = false;
+	std::string gtPath;
+	bool quiet = false;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+			else if(std::strcmp(argv[i], "--scan_step") == 0)
+			{
+				scanStep = atoi(argv[++i]);
+				if(scanStep <= 0)
+				{
+					printf("scan_step should be > 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_voxel") == 0)
+			{
+				scanVoxel = atof(argv[++i]);
+				if(scanVoxel < 0.0f)
+				{
+					printf("scan_voxel should be >= 0.0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_k") == 0)
+			{
+				scanNormalK = atoi(argv[++i]);
+				if(scanNormalK < 0)
+				{
+					printf("scanNormalK should be >= 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_radius") == 0)
+			{
+				scanNormalRadius = atof(argv[++i]);
+				if(scanNormalRadius < 0.0f)
+				{
+					printf("scanNormalRadius should be >= 0\n");
+					showUsage();
+				}
+			}
+			else if(std::strcmp(argv[i], "--scan_only") == 0)
+			{
+				scan = scanOnly = true;
+			}
+			else if(std::strcmp(argv[i], "--gt") == 0)
+			{
+				gtPath = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--color") == 0)
+			{
+				color = true;
+			}
+			else if(std::strcmp(argv[i], "--height") == 0)
+			{
+				height = true;
+			}
+			else if(std::strcmp(argv[i], "--scan") == 0)
+			{
+				scan = true;
+			}
+			else if(std::strcmp(argv[i], "--disp") == 0)
+			{
+				disp = true;
+			}
+			else if(std::strcmp(argv[i], "--exposure_comp") == 0)
+			{
+				exposureCompensation = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+	}
+
+	seq = uSplit(path, '/').back();
+	if(seq.empty() || !(uStr2Int(seq)>=0 && uStr2Int(seq)<=21))
+	{
+		UWARN("Sequence number \"%s\" should be between 0 and 21 (official KITTI datasets).", seq.c_str());
+		seq.clear();
+	}
+	std::string pathLeftImages  = path+(color?"/image_2":"/image_0");
+	std::string pathRightImages = path+(color?"/image_3":"/image_1");
+	std::string pathCalib = path+"/calib.txt";
+	std::string pathTimes = path+"/times.txt";
+	std::string pathScan;
+
+	printf("Paths:\n"
+			"   Sequence number:  %s\n"
+			"   Sequence path:    %s\n"
+			"   Output:           %s\n"
+			"   Output name:      %s\n"
+			"   left images:      %s\n"
+			"   right images:     %s\n"
+			"   calib.txt:        %s\n"
+			"   times.txt:        %s\n",
+			seq.c_str(),
+			path.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			pathLeftImages.c_str(),
+			pathRightImages.c_str(),
+			pathCalib.c_str(),
+			pathTimes.c_str());
+	if(!gtPath.empty())
+	{
+		gtPath = uReplaceChar(gtPath, '~', UDirectory::homeDir());
+		gtPath = uReplaceChar(gtPath, '\\', '/');
+		if(!UFile::exists(gtPath))
+		{
+			UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", gtPath.c_str());
+			gtPath.clear();
+		}
+		else
+		{
+			printf("   Ground Truth:      %s\n", gtPath.c_str());
+		}
+	}
+	printf("   Exposure Compensation: %s\n", exposureCompensation?"true":"false");
+	printf("   Disparity:         %s\n", disp?"true":"false");
+	if(scan)
+	{
+		pathScan = path+"/velodyne";
+		printf("   Scan:               %s\n", pathScan.c_str());
+		printf("   Scan only:          %s\n", scanOnly?"true":"false");
+		printf("   Scan step:          %d\n", scanStep);
+		printf("   Scan voxel:         %fm\n", scanVoxel);
+		printf("   Scan normal k:      %d\n", scanNormalK);
+		printf("   Scan normal radius: %f\n", scanNormalRadius);
+	}
+
+	// convert calib.txt to rtabmap format (yaml)
+	FILE * pFile = 0;
+	pFile = fopen(pathCalib.c_str(),"r");
+	if(!pFile)
+	{
+		UERROR("Cannot open calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	cv::Mat_<double> P0(3,4);
+	cv::Mat_<double> P1(3,4);
+	cv::Mat_<double> P2(3,4);
+	cv::Mat_<double> P3(3,4);
+	if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+			&P0(0, 0), &P0(0, 1), &P0(0, 2), &P0(0, 3),
+			&P0(1, 0), &P0(1, 1), &P0(1, 2), &P0(1, 3),
+			&P0(2, 0), &P0(2, 1), &P0(2, 2), &P0(2, 3)) != 12)
+	{
+		UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+			&P1(0, 0), &P1(0, 1), &P1(0, 2), &P1(0, 3),
+			&P1(1, 0), &P1(1, 1), &P1(1, 2), &P1(1, 3),
+			&P1(2, 0), &P1(2, 1), &P1(2, 2), &P1(2, 3)) != 12)
+	{
+		UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+			&P2(0, 0), &P2(0, 1), &P2(0, 2), &P2(0, 3),
+			&P2(1, 0), &P2(1, 1), &P2(1, 2), &P2(1, 3),
+			&P2(2, 0), &P2(2, 1), &P2(2, 2), &P2(2, 3)) != 12)
+	{
+		UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	if(fscanf (pFile, "%*s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+			&P3(0, 0), &P3(0, 1), &P3(0, 2), &P3(0, 3),
+			&P3(1, 0), &P3(1, 1), &P3(1, 2), &P3(1, 3),
+			&P3(2, 0), &P3(2, 1), &P3(2, 2), &P3(2, 3)) != 12)
+	{
+		UERROR("Failed to parse calibration file \"%s\"", pathCalib.c_str());
+		return -1;
+	}
+	fclose (pFile);
+	// get image size
+	UDirectory dir(pathLeftImages);
+	std::string firstImage = dir.getNextFileName();
+	cv::Mat image = cv::imread(dir.getNextFilePath());
+	if(image.empty())
+	{
+		UERROR("Failed to read first image of \"%s\"", firstImage.c_str());
+		return -1;
+	}
+
+	StereoCameraModel model(outputName+"_calib",
+			image.size(), P0.colRange(0,3), cv::Mat(), cv::Mat(), P0,
+			image.size(), P1.colRange(0,3), cv::Mat(), cv::Mat(), P1,
+			cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat());
+	if(!model.save(output, true))
+	{
+		UERROR("Could not save calibration!");
+		return -1;
+	}
+	printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
+
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	// We use CameraThread only to use postUpdate() method
+	Transform opticalRotation(0,0,1,0, -1,0,0,color?-0.06:0, 0,-1,0,height?1.67:0.0);
+	Camera * camera = 0;
+	if(scanOnly)
+	{
+		camera = new CameraImages(""); // Scan path is set below
+	}
+	else
+	{
+		camera = new CameraStereoImages(
+					pathLeftImages,
+					pathRightImages,
+					false, // assume that images are already rectified
+					0.0f);
+	}
+	SensorCaptureThread cameraThread(camera, parameters);
+	((CameraImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
+	if(exposureCompensation)
+	{
+		cameraThread.setStereoExposureCompensation(true);
+	}
+	if(disp)
+	{
+		cameraThread.setStereoToDepth(true);
+	}
+	if(!gtPath.empty())
+	{
+		((CameraImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
+	}
+	if(!pathScan.empty())
+	{
+		((CameraImages*)cameraThread.camera())->setScanPath(
+						pathScan,
+						130000,
+						Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f));
+		cameraThread.setScanParameters(
+								false,
+								scanStep,
+								0,
+								0,
+								scanVoxel,
+								scanNormalK,
+								scanNormalRadius,
+								0.8f);
+	}
+
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+
+	// assuming source is 10 Hz
+	int mapUpdate = detectionRate>0?10 / detectionRate:1;
+	if(mapUpdate < 1)
+	{
+		mapUpdate = 1;
+	}
+
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(output, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		SensorCaptureInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeData(&cameraInfo);
+		int iteration = 0;
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		cv::Mat covariance;
+		int odomKeyFrames = 0;
+		while(data.isValid() && g_forever)
+		{
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+
+			OdometryInfo odomInfo;
+			Transform pose = odom->process(data, &odomInfo);
+			float speed = 0.0f;
+			if(odomInfo.interval>0.0)
+				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+
+			bool processData = true;
+			if(iteration % mapUpdate != 0)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+			if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
+			{
+				covariance = odomInfo.reg.covariance;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
+				covariance = cv::Mat();
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(data.keypoints().size() == 0 && data.laserScanRaw().size())
+				{
+					if(rmse >= 0.0f)
+					{
+						//printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
+						//		iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				else
+				{
+					if(rmse >= 0.0f)
+					{
+						//printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
+						//		iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = SensorCaptureInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeData(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory ...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		rtabmap.getGraph(poses, links, true, true);
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!gtPath.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+			// compute KITTI statistics
+			float t_err = 0.0f;
+			float r_err = 0.0f;
+			graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), t_err, r_err);
+			printf("Ground truth comparison:\n");
+			printf("   KITTI t_err = %f %%\n", t_err);
+			printf("   KITTI r_err = %f deg/m\n", r_err);
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m\n", translational_rmse);
+			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
+
+			pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  KITTI t_err =         %f %%\n", t_err);
+			fprintf(pFile, "  KITTI r_err =         %f deg/m\n", r_err);
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}

rtabmap/tools/LidarViewer/CMakeLists.txt

@@ -0,0 +1,32 @@
+
+IF(NOT WITH_QT)
+   # visualization module required
+   FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
+ENDIF(NOT WITH_QT)
+
+SET(INCLUDE_DIRS
+    ${PROJECT_BINARY_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(lidar_viewer main.cpp)
+TARGET_LINK_LIBRARIES(lidar_viewer rtabmap_core rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( lidar_viewer 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_viewer)
+  
+INSTALL(TARGETS lidar_viewer
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/LidarViewer/main.cpp

@@ -0,0 +1,148 @@
+/*
+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.
+*/
+
+// Should be first on windows to avoid "WinSock.h has already been included" error
+#include <pcl/io/hdl_grabber.h>
+#include <pcl/io/vlp_grabber.h>
+
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <pcl/visualization/cloud_viewer.h>
+#include <stdio.h>
+#include <signal.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/common/time.h> //fps calculations
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/image_viewer.h>
+#include <pcl/console/parse.h>
+#include <rtabmap/core/lidar/LidarVLP16.h>
+
+using namespace pcl;
+using namespace pcl::console;
+using namespace pcl::visualization;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-lidar_viewer IP PORT driver\n"
+			"  driver       Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n");
+	exit(1);
+}
+
+// catch ctrl-c
+bool running = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	running = false;
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+	//ULogger::setPrintTime(false);
+	//ULogger::setPrintWhere(false);
+
+	int driver = 0;
+	std::string ip;
+	int port = 2368;
+	if(argc < 4)
+	{
+		showUsage();
+	}
+	else
+	{
+		ip = argv[1];
+		port = uStr2Int(argv[2]);
+		driver = atoi(argv[3]);
+		if(driver < 0 || driver > 0)
+		{
+			UERROR("driver should be 0.");
+			showUsage();
+		}
+	}
+	printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port);
+
+	rtabmap::LidarVLP16 * lidar = 0;
+	if(driver == 0)
+	{
+#if BOOST_VERSION >= 108700  // Version 1.87.0
+		lidar = new rtabmap::LidarVLP16(boost::asio::ip::make_address(ip), port);
+#else
+		lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+#endif
+	}
+	else
+	{
+		UFATAL("");
+	}
+
+	if(!lidar->init())
+	{
+		printf("Lidar init failed! Please select another driver (see \"--help\").\n");
+		delete lidar;
+		exit(1);
+	}
+
+	pcl::visualization::CloudViewer * viewer = new pcl::visualization::CloudViewer("cloud");
+
+	// to catch the ctrl-c
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	rtabmap::SensorData data = lidar->takeScan();
+	while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
+		viewer->showCloud(cloud, "cloud");
+		printf("Scan size: %ld points\n", cloud->size());
+
+		int c = cv::waitKey(10); // wait 10 ms or for key stroke
+		if(c == 27)
+			break; // if ESC, break and quit
+
+		data = lidar->takeScan();
+	}
+	printf("Closing...\n");
+	if(viewer)
+	{
+		delete viewer;
+	}
+	delete lidar;
+	return 0;
+}

rtabmap/tools/Matcher/CMakeLists.txt

@@ -0,0 +1,7 @@
+
+ADD_EXECUTABLE(matcher main.cpp)
+TARGET_LINK_LIBRARIES(matcher rtabmap_gui)
+
+SET_TARGET_PROPERTIES( matcher 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-matcher)
+ 

rtabmap/tools/Matcher/main.cpp

@@ -0,0 +1,645 @@
+/*
+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/Parameters.h>
+#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/core/EpipolarGeometry.h>
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_features.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/KeypointItem.h>
+#include <rtabmap/gui/CloudViewer.h>
+#include <rtabmap/utilite/UCv2Qt.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
+#include <fstream>
+#include <string>
+#include <QApplication>
+#include <QDialog>
+#include <QHBoxLayout>
+#include <QMultiMap>
+#include <QString>
+#include <opencv2/core/core.hpp>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\n\nUsage:\n"
+			"   rtabmap-matcher [Options] from.png to.png\n"
+			"Examples:\n"
+			"   rtabmap-matcher --Vis/CorNNType 5 --Vis/PnPReprojError 3 from.png to.png\n"
+			"   rtabmap-matcher --Vis/CorNNDR 0.8 from.png to.png\n"
+			"   rtabmap-matcher --Vis/FeatureType 11 --SuperPoint/ModelPath \"superpoint.pt\" --Vis/CorNNType 6 --PyMatcher/Path \"~/SuperGluePretrainedNetwork/rtabmap_superglue.py\" from.png to.png\n"
+			"   rtabmap-matcher --Vis/FeatureType 1 --Vis/CorNNType 6 --PyMatcher/Path \"~/OANet/demo/rtabmap_oanet.py\" --PyMatcher/Model \"~/OANet/model/gl3d/sift-4000/model_best.pth\" from.png to.png\n"
+			"   rtabmap-matcher --calibration calib.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
+			"   rtabmap-matcher --calibration calibFrom.yaml --calibration_to calibTo.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
+			"   rtabmap-matcher --calibration calib.yaml --Vis/FeatureType 2 --Vis/MaxFeatures 10000 --Vis/CorNNType 7 from.png to.png\n"
+			"\n"
+			"Note: Use \"Vis/\" parameters for feature stuff.\n"
+			"Options:\n"
+			"   --calibration \"calibration.yaml\" Calibration file. If not set, a\n"
+			"                                        fake one is created from image's\n"
+			"                                        size (which may not be optimal).\n"
+			"                                        Required if from_depth option is set.\n"
+			"                                        Assuming same calibration for both images\n"
+			"                                        if --calibration_to is not set.\n"
+			"   --calibration_to \"calibration.yaml\" Calibration file for \"to\" image. If not set,\n"
+			"                                           the same calibration of --calibration option is\n"
+			"                                           used for \"to\" image.\n"
+			"   --from_depth \"from_depth.png\"    Depth or right image file of the first image.\n"
+			"                                        If not set, 2D->2D estimation is done by \n"
+			"                                        default. For 3D->2D estimation, from_depth\n"
+			"                                        should be set.\n"
+			"   --to_depth \"to_depth.png\"        Depth or right image file of the second image.\n"
+			"                                        For 3D->3D estimation, from_depth and to_depth\n"
+			"                                        should be both set.\n"
+			"\n\n"
+			"%s\n",
+			Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 3)
+	{
+		showUsage();
+	}
+
+	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	std::string fromDepthPath;
+	std::string toDepthPath;
+	std::string calibrationPath;
+	std::string calibrationToPath;
+	for(int i=1; i<argc-2; ++i)
+	{
+		if(strcmp(argv[i], "--from_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				fromDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--to_depth") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				toDepthPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--calibration") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				calibrationPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--calibration_to") == 0)
+		{
+			++i;
+			if(i<argc-2)
+			{
+				calibrationToPath = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+
+	printf("Options\n");
+	printf("  --calibration = \"%s\"\n", calibrationPath.c_str());
+	if(!calibrationToPath.empty())
+	{
+		printf("  --calibration_to = \"%s\"\n", calibrationToPath.c_str());
+	}
+	printf("  --from_depth  = \"%s\"\n", fromDepthPath.c_str());
+	printf("  --to_depth    = \"%s\"\n", toDepthPath.c_str());
+
+#ifdef RTABMAP_PYTHON
+	rtabmap::PythonInterface pythonInterface;
+#endif
+
+	ParametersMap parameters = Parameters::parseArguments(argc, argv);
+	parameters.insert(ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+
+	cv::Mat imageFrom = cv::imread(argv[argc-2], cv::IMREAD_COLOR);
+	cv::Mat imageTo = cv::imread(argv[argc-1], cv::IMREAD_COLOR);
+
+	if(!imageFrom.empty() && !imageTo.empty())
+	{
+		//////////////////
+		// Load data
+		//////////////////
+
+		cv::Mat fromDepth;
+		cv::Mat toDepth;
+		if(!calibrationPath.empty())
+		{
+			if(!fromDepthPath.empty())
+			{
+				fromDepth = cv::imread(fromDepthPath, cv::IMREAD_UNCHANGED);
+				if(fromDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(fromDepth, fromDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(fromDepth.empty())
+				{
+					printf("Failed loading from_depth image: \"%s\"!", fromDepthPath.c_str());
+				}
+			}
+			if(!toDepthPath.empty())
+			{
+				toDepth = cv::imread(toDepthPath, cv::IMREAD_UNCHANGED);
+				if(toDepth.type() == CV_8UC3)
+				{
+					cv::cvtColor(toDepth, toDepth, cv::COLOR_BGR2GRAY);
+				}
+				else if(toDepth.empty())
+				{
+					printf("Failed loading to_depth image: \"%s\"!", toDepthPath.c_str());
+				}
+			}
+			UASSERT(toDepth.empty() || (!fromDepth.empty() && fromDepth.type() == toDepth.type()));
+		}
+		else if(!fromDepthPath.empty() || !fromDepthPath.empty())
+		{
+			printf("A calibration file should be provided if depth images are used!\n");
+			showUsage();
+		}
+
+		CameraModel model;
+		StereoCameraModel stereoModel;
+		CameraModel modelTo;
+		StereoCameraModel stereoModelTo;
+		if(!fromDepth.empty())
+		{
+			if(fromDepth.type() != CV_8UC1)
+			{
+				if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(calibrationToPath.empty())
+				{
+					modelTo = model;
+				}
+			}
+			else // fromDepth.type() == CV_8UC1
+			{
+				if(!stereoModel.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+					exit(-1);
+				}
+				if(calibrationToPath.empty())
+				{
+					stereoModelTo = stereoModel;
+				}
+			}
+			if(!calibrationToPath.empty())
+			{
+				if(toDepth.empty() || toDepth.type() != CV_8UC1)
+				{
+					if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+					{
+						printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+						exit(-1);
+					}
+				}
+				else // toDepth.type() == CV_8UC1
+				{
+					if(!stereoModelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+					{
+						printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+						exit(-1);
+					}
+				}
+			}
+		}
+		else if(!calibrationPath.empty())
+		{
+			if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
+			{
+				printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
+				exit(-1);
+			}
+			if(!calibrationToPath.empty())
+			{
+				if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
+				{
+					printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
+					exit(-1);
+				}
+			}
+			else
+			{
+				modelTo = model;
+			}
+		}
+		else
+		{
+			printf("Using fake calibration model \"from\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+					imageFrom.cols, imageFrom.rows, imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2);
+			model = CameraModel(imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2); // Fake model
+			model.setImageSize(imageFrom.size());
+			printf("Using fake calibration model \"to\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
+					imageTo.cols, imageTo.rows, imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2);
+			modelTo = CameraModel(imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2); // Fake model
+			modelTo.setImageSize(imageTo.size());
+		}
+
+		Signature dataFrom;
+		Signature dataTo;
+		if(model.isValidForProjection())
+		{
+			printf("Mono calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, model, 1);
+			dataTo = SensorData(imageTo, toDepth, modelTo, 2);
+		}
+		else //stereo
+		{
+			printf("Stereo calibration model detected.\n");
+			dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
+			dataTo = SensorData(imageTo, toDepth, stereoModelTo, 2);
+		}
+
+		//////////////////
+		// Registration
+		//////////////////
+
+		if(fromDepth.empty())
+		{
+			parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
+			parameters.insert(ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); //Unknown scale
+			printf("Calibration not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
+		}
+		RegistrationVis reg(parameters);
+		RegistrationInfo info;
+
+
+		// Do it one time before to make sure everything is loaded to get realistic timing for matching only.
+		reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+
+		UTimer timer;
+		Transform t = reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
+		double matchingTime = timer.ticks();
+		printf("Time matching and motion estimation (excluding feature detection): %fs\n", matchingTime);
+
+		//////////////////
+		// Visualization
+		//////////////////
+
+		if(reg.getNNType()==6 &&
+		   !dataFrom.getWordsDescriptors().empty() &&
+		   dataFrom.getWordsDescriptors().type()!=CV_32F)
+		{
+			UWARN("PyMatcher is selected for matching but binary features "
+				  "are not compatible. BruteForce with CrossCheck (%s=5) "
+				  "has been used instead.", Parameters::kVisCorNNType().c_str());
+		}
+
+		QApplication app(argc, argv);
+		QDialog dialog;
+		float reprojError = Parameters::defaultVisPnPReprojError();
+		std::string pyMatcherPath;
+		Parameters::parse(parameters, Parameters::kVisPnPReprojError(), reprojError);
+		Parameters::parse(parameters, Parameters::kPyMatcherPath(), pyMatcherPath);
+		dialog.setWindowTitle(QString("Matches (%1/%2) %3 sec [%4=%5 (%6) %7=%8 (%9)%10 %11=%12 (%13) %14=%15]")
+				.arg(info.inliers)
+				.arg(info.matches)
+				.arg(matchingTime)
+				.arg(Parameters::kVisFeatureType().c_str())
+				.arg(reg.getDetector()?reg.getDetector()->getType():-1)
+				.arg(reg.getDetector()?Feature2D::typeName(reg.getDetector()->getType()).c_str():"?")
+				.arg(Parameters::kVisCorNNType().c_str())
+				.arg(reg.getNNType())
+				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
+						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().type()!=CV_32F)?"BFCrossCheck":
+						reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
+						reg.getNNType()==7?"GMS":"?")
+				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
+				.arg(Parameters::kVisEstimationType().c_str())
+				.arg(reg.getEstimationType())
+				.arg(reg.getEstimationType()==0?"3D->3D":reg.getEstimationType()==1?"3D->2D":reg.getEstimationType()==2?"2D->2D":"?")
+				.arg(Parameters::kVisPnPReprojError().c_str())
+				.arg(reprojError));
+
+		CloudViewer * viewer = 0;
+		if(!t.isNull())
+		{
+			viewer = new CloudViewer(&dialog);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom = util3d::cloudRGBFromSensorData(dataFrom.sensorData());
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo = util3d::cloudRGBFromSensorData(dataTo.sensorData());
+			viewer->addCloud(uFormat("cloud_%d", dataFrom.id()), cloudFrom, Transform::getIdentity(), Qt::magenta);
+			viewer->addCloud(uFormat("cloud_%d", dataTo.id()), cloudTo, t, Qt::cyan);
+			viewer->addOrUpdateCoordinate(uFormat("frame_%d", dataTo.id()), t, 0.2);
+			viewer->setGridShown(true);
+
+			if(reg.getEstimationType() == 2)
+			{
+				// triangulate 3D words based on the transform computed
+				std::map<int, int> wordsFrom = uMultimapToMapUnique(dataFrom.getWords());
+				std::map<int, int> wordsTo = uMultimapToMapUnique(dataTo.getWords());
+				std::map<int, cv::KeyPoint> kptsFrom;
+				std::map<int, cv::KeyPoint> kptsTo;
+				for(std::map<int, int>::iterator iter=wordsFrom.begin(); iter!=wordsFrom.end(); ++iter)
+				{
+					kptsFrom.insert(std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
+				}
+				for(std::map<int, int>::iterator iter=wordsTo.begin(); iter!=wordsTo.end(); ++iter)
+				{
+					kptsTo.insert(std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
+				}
+				std::map<int, cv::Point3f> points3d = util3d::generateWords3DMono(
+						kptsFrom,
+						kptsTo,
+						model.isValidForProjection()?model:stereoModel.left(),
+						t);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+				cloudWordsFrom->resize(points3d.size());
+				int i=0;
+				for(std::multimap<int, cv::Point3f>::const_iterator iter=points3d.begin();
+					iter!=points3d.end();
+					++iter)
+				{
+					cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+				}
+				if(cloudWordsFrom->size())
+				{
+					cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+				}
+				if(cloudWordsFrom->size())
+				{
+					viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::yellow);
+					viewer->setCloudPointSize("wordsFrom", 5);
+				}
+			}
+			else
+			{
+				if(!dataFrom.getWords3().empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+					cloudWordsFrom->resize(dataFrom.getWords3().size());
+					int i=0;
+					for(std::multimap<int, int>::const_iterator iter=dataFrom.getWords().begin();
+						iter!=dataFrom.getWords().end();
+						++iter)
+					{
+						const cv::Point3f & pt = dataFrom.getWords3()[iter->second];
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+					}
+					if(cloudWordsFrom->size())
+					{
+						cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+					}
+					if(cloudWordsFrom->size())
+					{
+						viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::magenta);
+						viewer->setCloudPointSize("wordsFrom", 5);
+					}
+				}
+				if(!dataTo.getWords3().empty())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsTo(new pcl::PointCloud<pcl::PointXYZ>);
+					cloudWordsTo->resize(dataTo.getWords3().size());
+					int i=0;
+					for(std::multimap<int, int>::const_iterator iter=dataTo.getWords().begin();
+						iter!=dataTo.getWords().end();
+						++iter)
+					{
+						const cv::Point3f & pt = dataTo.getWords3()[iter->second];
+						cloudWordsTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+					}
+					if(cloudWordsTo->size())
+					{
+						cloudWordsTo = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsTo);
+					}
+					if(cloudWordsTo->size())
+					{
+						viewer->addCloud("wordsTo", cloudWordsTo, t, Qt::cyan);
+						viewer->setCloudPointSize("wordsTo", 5);
+					}
+				}
+			}
+		}
+
+		QBoxLayout * mainLayout = new QHBoxLayout();
+		mainLayout->setContentsMargins(0, 0, 0, 0);
+		mainLayout->setSpacing(0);
+		QBoxLayout * layout;
+		bool vertical=true;
+		if(imageFrom.cols > imageFrom.rows)
+		{
+			dialog.setMinimumWidth(640*(viewer?2:1));
+			dialog.setMinimumHeight(640*imageFrom.rows/imageFrom.cols*2);
+			layout = new QVBoxLayout();
+		}
+		else
+		{
+			dialog.setMinimumWidth((640*imageFrom.cols/imageFrom.rows*2)*(viewer?2:1));
+			dialog.setMinimumHeight(640);
+			layout = new QHBoxLayout();
+			vertical = false;
+		}
+
+		ImageView * viewA = new ImageView(&dialog);
+		ImageView * viewB = new ImageView(&dialog);
+
+		layout->setSpacing(0);
+		layout->addWidget(viewA, 1);
+		layout->addWidget(viewB, 1);
+
+		mainLayout->addLayout(layout, 1);
+		if(viewer)
+		{
+			mainLayout->addWidget(viewer, 1);
+		}
+
+		dialog.setLayout(mainLayout);
+
+		dialog.show();
+
+		viewA->setImage(uCvMat2QImage(imageFrom));
+		viewA->setAlpha(200);
+		if(!fromDepth.empty())
+		{
+			viewA->setImageDepth(uCvMat2QImage(fromDepth, false, uCvQtDepthRedToBlue));
+			viewA->setImageDepthShown(true);
+		}
+		viewB->setImage(uCvMat2QImage(imageTo));
+		viewB->setAlpha(200);
+		if(!toDepth.empty())
+		{
+			viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
+			viewB->setImageDepthShown(true);
+		}
+		std::multimap<int, cv::KeyPoint> keypointsFrom;
+		std::multimap<int, cv::KeyPoint> keypointsTo;
+		if(!dataFrom.getWordsKpts().empty())
+		{
+			for(std::map<int, int>::const_iterator iter=dataFrom.getWords().begin(); iter!=dataFrom.getWords().end(); ++iter)
+			{
+				keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
+			}
+		}
+		if(!dataTo.getWordsKpts().empty())
+		{
+			for(std::map<int, int>::const_iterator iter=dataTo.getWords().begin(); iter!=dataTo.getWords().end(); ++iter)
+			{
+				keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
+			}
+		}
+		viewA->setFeatures(keypointsFrom);
+		viewB->setFeatures(keypointsTo);
+		std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
+
+		const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();
+		const QMultiMap<int, KeypointItem*> & wordsB = viewB->getFeatures();
+		if(wordsA.size() && wordsB.size())
+		{
+			QList<int> ids = wordsA.uniqueKeys();
+			for(int i=0; i<ids.size(); ++i)
+			{
+				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
+				{
+					// Add lines
+					// Draw lines between corresponding features...
+					float scaleAX = viewA->viewScale();
+					float scaleBX = viewB->viewScale();
+
+					float scaleDiff = viewA->viewScale() / viewB->viewScale();
+					float deltaAX = 0;
+					float deltaAY = 0;
+
+					if(vertical)
+					{
+						deltaAY = viewA->height()/scaleAX;
+					}
+					else
+					{
+						deltaAX = viewA->width()/scaleAX;
+					}
+
+					float deltaBX = 0;
+					float deltaBY = 0;
+
+					if(vertical)
+					{
+						deltaBY = viewB->height()/scaleBX;
+					}
+					else
+					{
+						deltaBX = viewA->width()/scaleBX;
+					}
+
+					const KeypointItem * kptA = wordsA.value(ids[i]);
+					const KeypointItem * kptB = wordsB.value(ids[i]);
+
+					QColor cA = viewA->getDefaultMatchingLineColor();
+					QColor cB = viewB->getDefaultMatchingLineColor();
+					if(inliersSet.find(ids[i])!=inliersSet.end())
+					{
+						cA = viewA->getDefaultMatchingFeatureColor();
+						cB = viewB->getDefaultMatchingFeatureColor();
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingFeatureColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingFeatureColor());
+					}
+					else
+					{
+						viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingLineColor());
+						viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingLineColor());
+					}
+
+					viewA->addLine(
+							kptA->rect().x()+kptA->rect().width()/2,
+							kptA->rect().y()+kptA->rect().height()/2,
+							kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
+							kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
+							cA);
+
+					viewB->addLine(
+							kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
+							kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
+							kptB->rect().x()+kptB->rect().width()/2,
+							kptB->rect().y()+kptB->rect().height()/2,
+							cB);
+				}
+			}
+			viewA->update();
+			viewB->update();
+		}
+
+		printf("Transform: %s\n", t.prettyPrint().c_str());
+		printf("Features: from=%d to=%d\n", (int)dataFrom.getWords().size(), (int)dataTo.getWords().size());
+		printf("Matches: %d\n", info.matches);
+		printf("Inliers: %d (%s=%d)\n", info.inliers, Parameters::kVisMinInliers().c_str(), reg.getMinInliers());
+		app.exec();
+		delete viewer;
+	}
+	else
+	{
+		printf("Failed loading images %s and %s\n!", argv[argc-2], argv[argc-1]);
+	}
+
+
+	return 0;
+}
+

rtabmap/tools/OdometryViewer/CMakeLists.txt

@@ -0,0 +1,15 @@
+
+IF(MINGW)
+  ADD_EXECUTABLE(odometryViewer WIN32 main.cpp)
+ELSE()
+  ADD_EXECUTABLE(odometryViewer main.cpp)
+ENDIF()
+TARGET_LINK_LIBRARIES(odometryViewer rtabmap_gui)
+
+SET_TARGET_PROPERTIES( odometryViewer 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-odometryViewer)
+  
+INSTALL(TARGETS odometryViewer
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+

rtabmap/tools/OdometryViewer/main.cpp

@@ -0,0 +1,416 @@
+/*
+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/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/odometry/OdometryMono.h>
+#include <rtabmap/core/OdometryThread.h>
+#include <rtabmap/gui/OdometryViewer.h>
+#include <rtabmap/core/CameraRGBD.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <QApplication>
+#include <QPushButton>
+#include <pcl/console/print.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"odometryViewer [options]\n"
+			"Options:\n"
+			"  -driver #                 Driver number to use: \n"
+			"                                     0=OpenNI-PCL (Kinect)\n"
+			"                                     1=OpenNI2    (Kinect and Xtion PRO Live)\n"
+			"                                     2=Freenect   (Kinect)\n"
+			"                                     3=OpenNI-CV  (Kinect)\n"
+			"                                     4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
+			"                                     5=Freenect2  (Kinect v2)\n"
+			"                                     6=DC1394     (Bumblebee2)\n"
+			"                                     7=FlyCapture2 (Bumblebee2)\n"
+			"                                     8=ZED stereo\n"
+			"                                     9=RealSense\n"
+			"                                     10=Kinect for Windows 2 SDK\n"
+			"                                     11=RealSense2\n"
+			"                                     12=Kinect for Azure SDK\n"
+			"                                     13=MYNT EYE S\n"
+			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
+			"  -db \"input.db\"            Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
+			"  -clouds #                 Maximum clouds shown (default 10, zero means inf)\n"
+			"  -sec #.#                  Delay (seconds) before reading the database (if set)\n"
+			"%s\n",
+			rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+int main (int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	// parse arguments
+	float rate = 0.0;
+	std::string inputDatabase;
+	int driver = 0;
+	int maxClouds = 10;
+	float sec = 0.0f;
+
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-driver") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				driver = std::atoi(argv[i]);
+				if(driver < 0 || driver > 13)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-hz") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = uStr2Float(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-db") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				inputDatabase = argv[i];
+				if(UFile::getExtension(inputDatabase).compare("db") != 0)
+				{
+					printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-clouds") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				maxClouds = std::atoi(argv[i]);
+				if(maxClouds < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-sec") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				sec = uStr2Float(argv[i]);
+				if(sec < 0.0f)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+
+	if(inputDatabase.size())
+	{
+		UINFO("Using database input \"%s\"", inputDatabase.c_str());
+	}
+	else
+	{
+		UINFO("Using OpenNI camera");
+	}
+
+	UINFO("Camera rate =             %f Hz", rate);
+	UINFO("Maximum clouds shown =    %d", maxClouds);
+	UINFO("Delay =                   %f s", sec);
+
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		UINFO(" Param \"%s\"=\"%s\"", iter->first.c_str(), iter->second.c_str());
+	}
+
+	bool icp = false;
+	int regStrategy = rtabmap::Parameters::defaultRegStrategy();
+	int odomStrategy = rtabmap::Parameters::defaultOdomStrategy();
+	rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kRegStrategy(), regStrategy);
+	rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomStrategy(), odomStrategy);
+	int decimation = 8;
+	float maxDepth = 4.0f;
+	float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
+	int normalsK = 0;
+	float normalsRadius = 0.0f;
+	if(regStrategy == 1 || regStrategy == 2)
+	{
+		// icp requires scans
+		icp = true;
+
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpDownsamplingStep(), decimation);
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpVoxelSize(), voxelSize);
+
+		bool pointToPlane = rtabmap::Parameters::defaultIcpPointToPlane();
+		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
+		if(pointToPlane)
+		{
+			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
+			normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
+		}
+
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpVoxelSize(), "0"));
+	}
+
+	QApplication app(argc, argv);
+
+	rtabmap::Odometry * odom;
+	if(odomStrategy == -1)
+	{
+		// experimental mono
+		odom = new rtabmap::OdometryMono(parameters);
+	}
+	else
+	{
+		odom = rtabmap::Odometry::create(parameters);
+	}
+
+
+	rtabmap::OdometryThread odomThread(odom);
+	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
+	UEventsManager::addHandler(&odomThread);
+	UEventsManager::addHandler(&odomViewer);
+
+	odomViewer.setWindowTitle("Odometry view");
+	odomViewer.resize(1280, 480+QPushButton().minimumHeight());
+
+	rtabmap::Camera * camera = 0;
+	if(inputDatabase.size())
+	{
+		camera =  new rtabmap::DBReader(inputDatabase, rate, true);
+	}
+	else if(driver == 0)
+	{
+		camera = new rtabmap::CameraOpenni("", rate);
+	}
+	else if(driver == 1)
+	{
+		if(!rtabmap::CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate);
+	}
+	else if(driver == 2)
+	{
+		if(!rtabmap::CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate);
+	}
+	else if(driver == 3)
+	{
+		if(!rtabmap::CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNICV(false, rate);
+	}
+	else if(driver == 4)
+	{
+		if(!rtabmap::CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraOpenNICV(true, rate);
+	}
+	else if(driver == 5)
+	{
+		if(!rtabmap::CameraFreenect2::available())
+		{
+			UERROR("Not built with Freenect2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColor2DepthSD, rate);
+	}
+	else if(driver == 6)
+	{
+		if(!rtabmap::CameraStereoDC1394::available())
+		{
+			UERROR("Not built with dc1394 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoDC1394(rate);
+	}
+	else if(driver == 7)
+	{
+		if(!rtabmap::CameraStereoFlyCapture2::available())
+		{
+			UERROR("Not built with FlyCapture2/Triclops support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoFlyCapture2(rate);
+	}
+	else if(driver == 8)
+	{
+		if(!rtabmap::CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED sdk support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZed(0,-1,1,0,100,false,rate);
+	}
+	else if (driver == 9)
+	{
+		if (!rtabmap::CameraRealSense::available())
+		{
+			UERROR("Not built with RealSense support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense(0, 0, 0, false, rate);
+	}
+	else if (driver == 10)
+	{
+		if (!rtabmap::CameraK4W2::available())
+		{
+			UERROR("Not built with Kinect for Windows 2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4W2(0, rtabmap::CameraK4W2::kTypeDepth2ColorSD, rate);
+	}
+	else if (driver == 11)
+	{
+		if (!rtabmap::CameraRealSense2::available())
+		{
+			UERROR("Not built with RealSense2 SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraRealSense2("", rate);
+	}
+	else if (driver == 12)
+	{
+		if (!rtabmap::CameraK4A::available())
+		{
+			UERROR("Not built with Kinect for Azure SDK support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraK4A();
+	}
+	else if (driver == 13)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye("", false, false, rate);
+	}
+	else
+	{
+		UFATAL("Camera driver (%d) not found!", driver);
+	}
+
+	//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
+
+	if(camera->init())
+	{
+		if(camera->isCalibrated())
+		{
+			rtabmap::SensorCaptureThread cameraThread(camera, parameters);
+
+			cameraThread.setScanParameters(icp, decimation<1?1:decimation, 0, maxDepth, voxelSize, normalsK, normalsRadius);
+
+			odomThread.start();
+			cameraThread.start();
+
+			odomViewer.exec();
+
+			cameraThread.join(true);
+			odomThread.join(true);
+		}
+		else
+		{
+			printf("The camera is not calibrated! You should calibrate the camera first.\n");
+			delete camera;
+		}
+	}
+	else
+	{
+		printf("Failed to initialize the camera! Please select another driver (see \"--help\").\n");
+		delete camera;
+	}
+
+	return 0;
+}

rtabmap/tools/Recovery/CMakeLists.txt

@@ -0,0 +1,14 @@
+
+ADD_EXECUTABLE(recovery main.cpp)
+  
+TARGET_LINK_LIBRARIES(recovery rtabmap_core)
+
+SET_TARGET_PROPERTIES( recovery 
+	PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-recovery)
+
+INSTALL(TARGETS recovery
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
+

rtabmap/tools/Recovery/main.cpp

@@ -0,0 +1,99 @@
+/*
+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/Recovery.h>
+#include <rtabmap/core/ProgressState.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-recovery [-d] \"my_corrupted_map.db\""
+			"  Options:\n"
+			"     -d        Delete database backup on success (\"*.backup.db\").\n"
+			"\n");
+	exit(1);
+}
+
+class RecoveryProgressState: public ProgressState
+{
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			printf("%s\n", msg.c_str());
+		return true;
+	}
+};
+RecoveryProgressState state;
+
+// catch ctrl-c
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	state.setCanceled(true);
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	bool keepBackup = true;
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-d") == 0)
+		{
+			keepBackup = false;
+		}
+	}
+
+	std::string databasePath = argv[argc-1];
+
+	std::string errorMsg;
+	printf("Recovering \"%s\"\n", databasePath.c_str());
+	if(!databaseRecovery(databasePath, keepBackup, &errorMsg, &state))
+	{
+		printf("Error: %s\n", errorMsg.c_str());
+		return 1;
+	}
+
+	return 0;
+}

rtabmap/tools/Report/CMakeLists.txt

@@ -0,0 +1,18 @@
+
+set(LIBRARIES rtabmap_core)
+
+IF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND))
+    ADD_DEFINITIONS("-DWITH_QT")
+    set(LIBRARIES ${LIBRARIES} rtabmap_gui)
+ENDIF(WITH_QT AND (QT4_FOUND OR Qt5_FOUND))
+
+ADD_EXECUTABLE(report main.cpp)
+  
+TARGET_LINK_LIBRARIES(report ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( report 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-report)
+    
+INSTALL(TARGETS report
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/Reprocess/CMakeLists.txt

@@ -0,0 +1,14 @@
+
+ADD_EXECUTABLE(reprocess main.cpp)
+  
+TARGET_LINK_LIBRARIES(reprocess rtabmap_core)
+
+SET_TARGET_PROPERTIES( reprocess 
+	PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reprocess)
+
+INSTALL(TARGETS reprocess
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
+

rtabmap/tools/RgbdDataset/CMakeLists.txt

@@ -0,0 +1,11 @@
+
+ADD_EXECUTABLE(rgbd_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(rgbd_dataset rtabmap_core)
+
+SET_TARGET_PROPERTIES( rgbd_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_dataset)
+    
+INSTALL(TARGETS rgbd_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

rtabmap/tools/RgbdDataset/main.cpp

@@ -0,0 +1,496 @@
+/*
+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/CameraRGBD.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include <pcl/common/common.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/rgbd_dataset_freiburg3_long_office_household\")\n"
+			"                        containing least rgb_sync and depth_sync folders. These folders contain\n"
+			"                        synchronized images using associate.py tool (use tool version from\n"
+			"                        https://gist.github.com/matlabbe/484134a2d9da8ad425362c6669824798). If \n"
+			"                        \"groundtruth.txt\" is found in the sequence folder, they will be saved in the database.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --skip #           Skip X frames.\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-rgbd_dataset \\\n"
+			"       --Rtabmap/PublishRAMUsage true\\\n"
+			"       --Rtabmap/DetectionRate 2\\\n"
+			"       --RGBD/LinearUpdate 0\\\n"
+			"       --Mem/STMSize 30\\\n"
+			"       ~/rgbd_dataset_freiburg3_long_office_household\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	int skipFrames = 0;
+	bool quiet = false;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--skip") == 0)
+			{
+				skipFrames = atoi(argv[++i]);
+				UASSERT(skipFrames > 0);
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+	}
+
+	std::string seq = uSplit(path, '/').back();
+	std::string pathRgbImages  = path+"/rgb_sync";
+	std::string pathDepthImages = path+"/depth_sync";
+	std::string pathGt = path+"/groundtruth.txt";
+	if(!UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	printf("Paths:\n"
+			"   Dataset name:    %s\n"
+			"   Dataset path:    %s\n"
+			"   RGB path:        %s\n"
+			"   Depth path:      %s\n"
+			"   Output:          %s\n"
+			"   Output name:     %s\n"
+			"   Skip frames:     %d\n",
+			seq.c_str(),
+			path.c_str(),
+			pathRgbImages.c_str(),
+			pathDepthImages.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			skipFrames);
+	if(!pathGt.empty())
+	{
+		printf("   groundtruth.txt: %s\n", pathGt.c_str());
+	}
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	// setup calibration file
+	CameraModel model;
+	std::string sequenceName = UFile(path).getName();
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	float depthFactor = 5.0f;
+	if(sequenceName.find("freiburg1") != std::string::npos)
+	{
+		model = CameraModel(outputName+"_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
+	}
+	else if(sequenceName.find("freiburg2") != std::string::npos)
+	{
+		model = CameraModel(outputName+"_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
+	}
+	else //if(sequenceName.find("freiburg3") != std::string::npos)
+	{
+		model = CameraModel(outputName+"_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
+	}
+	//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
+	model.save(path);
+
+	SensorCaptureThread cameraThread(new
+		CameraRGBDImages(
+				pathRgbImages,
+				pathDepthImages,
+				depthFactor,
+				0.0f), parameters);
+	((CameraRGBDImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(!pathGt.empty())
+	{
+		((CameraRGBDImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 1);
+	}
+
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(path, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
+
+		if(skipFrames>0)
+		{
+			totalImages /= skipFrames+1;
+		}
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		SensorCaptureInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeData(&cameraInfo);
+		int iteration = 0;
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		int odomKeyFrames = 0;
+		double previousStamp = 0.0;
+		int skipCount = 0;
+		while(data.isValid() && g_forever)
+		{
+			if(skipCount < skipFrames)
+			{
+				++skipCount;
+
+				cameraInfo = SensorCaptureInfo();
+				timer.restart();
+				data = cameraThread.camera()->takeData(&cameraInfo);
+				continue;
+			}
+			skipCount = 0;
+
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			OdometryInfo odomInfo;
+			Transform pose = odom->process(data, &odomInfo);
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+			
+			if(iteration!=0 && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0)>=9999)
+            {
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at<double>(0,0));
+				rtabmap.triggerNewMap();
+            }
+
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			bool processData = true;
+			if(detectionRate>0.0f &&
+				previousStamp>0.0 &&
+				data.stamp()>previousStamp && data.stamp() - previousStamp < 1.0/detectionRate)
+			{
+				processData = false;
+			}
+
+			if(processData)
+			{
+				previousStamp = data.stamp();
+			}
+
+			if(!processData)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/HistogramEqualization/ms", cameraInfo.timeHistogramEqualization*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats);
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(rmse >= 0.0f)
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+				}
+				else
+				{
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = SensorCaptureInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeData(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		std::map<int, Signature> signatures;
+		std::map<int, double> stamps;
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors sensors;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, sensors, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m\n", translational_rmse);
+			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
+
+			FILE * pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}

rtabmap/tools/StereoEval/CMakeLists.txt

@@ -0,0 +1,7 @@
+
+ADD_EXECUTABLE(stereoEval main.cpp)
+TARGET_LINK_LIBRARIES(stereoEval rtabmap_core)
+
+SET_TARGET_PROPERTIES( stereoEval 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-stereoEval)
+ 

rtabmap/tools/StereoEval/io.h

@@ -0,0 +1,180 @@
+/*
+ * pfmReader.h
+ *
+ *  Created on: Dec 4, 2015
+ *      Author: mathieu
+ */
+
+#ifndef PFMREADER_H_
+#define PFMREADER_H_
+
+#include <iostream>
+#include <stdio.h>
+#include <opencv2/opencv.hpp>
+
+// taken from http://vision.middlebury.edu/stereo/ evaluation tool
+
+void skipComment(FILE *fp)
+{
+    // skip comment lines in the headers of pnm files
+
+    char c;
+    while ((c=getc(fp)) == '#')
+        while (getc(fp) != '\n') ;
+    ungetc(c, fp);
+}
+
+void skipSpace(FILE *fp)
+{
+    // skip white space in the headers or pnm files
+
+    char c;
+    do {
+        c = getc(fp);
+    } while (c == '\n' || c == ' ' || c == '\t' || c == '\r');
+    ungetc(c, fp);
+}
+
+bool readHeader(FILE *fp, const char *imtype, char c1, char c2,
+                 int *width, int *height, int *nbands, int thirdArg)
+{
+    // read the header of a pnmfile and initialize width and height
+
+    char c;
+
+	if (getc(fp) != c1 || getc(fp) != c2)
+	{
+		printf("ReadFilePGM: wrong magic code for %s file\n", imtype);
+		return false;
+	}
+	skipSpace(fp);
+	skipComment(fp);
+	skipSpace(fp);
+	int r = fscanf(fp, "%d", width);
+	if(r==0)
+	{
+		return false;
+	}
+	skipSpace(fp);
+	r = fscanf(fp, "%d", height);
+	if(r==0)
+	{
+		return false;
+	}
+	if (thirdArg) {
+		skipSpace(fp);
+		r = fscanf(fp, "%d", nbands);
+		if(r==0)
+		{
+			return false;
+		}
+	}
+    // skip SINGLE newline character after reading image height (or third arg)
+	c = getc(fp);
+    if (c == '\r')      // <cr> in some files before newline
+        c = getc(fp);
+    if (c != '\n') {
+        if (c == ' ' || c == '\t' || c == '\r')
+        {
+            printf("newline expected in file after image height\n");
+            return false;
+        }
+        else
+        {
+        	printf("whitespace expected in file after image height\n");
+        	return false;
+        }
+  }
+
+    return true;
+}
+
+int littleendian()
+{
+    int intval = 1;
+    uchar *uval = (uchar *)&intval;
+    return uval[0] == 1;
+}
+
+cv::Mat readPFM(const char* filename)
+{
+	cv::Mat disp;
+
+    // Open the file and read the header
+    FILE *fp = fopen(filename, "rb");
+    if (fp == 0)
+    {
+        printf("ReadFilePFM: could not open %s\n", filename);
+        return cv::Mat();
+    }
+
+    int width, height, nBands;
+    readHeader(fp, "PFM", 'P', 'f', &width, &height, &nBands, 0);
+
+    skipSpace(fp);
+
+    float scalef;
+    int r = fscanf(fp, "%f", &scalef);  // scale factor (if negative, little endian)
+    if(r==0)
+	{
+    	return cv::Mat();
+	}
+
+    // skip SINGLE newline character after reading third arg
+    char c = getc(fp);
+    if (c == '\r')      // <cr> in some files before newline
+        c = getc(fp);
+    if (c != '\n') {
+        if (c == ' ' || c == '\t' || c == '\r')
+        {
+            printf("newline expected in file after scale factor\n");
+            return cv::Mat();
+        }
+        else
+        {
+            printf("whitespace expected in file after scale factor\n");
+            return cv::Mat();
+        }
+    }
+
+    // Set the image shape
+    disp = cv::Mat(height, width, CV_32FC1);
+
+    int littleEndianFile = (scalef < 0);
+    int littleEndianMachine = littleendian();
+    int needSwap = (littleEndianFile != littleEndianMachine);
+    //printf("endian file = %d, endian machine = %d, need swap = %d\n",
+    //       littleEndianFile, littleEndianMachine, needSwap);
+
+    for (int y = height-1; y >= 0; y--) { // PFM stores rows top-to-bottom!!!!
+	int n = width;
+	float* ptr = (float *) disp.row(y).data;
+	if ((int)fread(ptr, sizeof(float), n, fp) != n)
+	{
+	    printf("ReadFilePFM(%s): file is too short\n", filename);
+	    return cv::Mat();
+	}
+
+	if (needSwap) { // if endianness doesn't agree, swap bytes
+	    uchar* ptr = (uchar *) disp.row(y).data;
+	    int x = 0;
+	    uchar tmp = 0;
+	    while (x < n) {
+		tmp = ptr[0]; ptr[0] = ptr[3]; ptr[3] = tmp;
+		tmp = ptr[1]; ptr[1] = ptr[2]; ptr[2] = tmp;
+		ptr += 4;
+		x++;
+	    }
+	}
+    }
+    if (fclose(fp))
+    {
+        printf("ReadFilePGM(%s): error closing file\n", filename);
+        return cv::Mat();
+    }
+
+    return disp;
+}
+
+
+#endif /* PFMREADER_H_ */

rtabmap/tools/StereoEval/main.cpp

@@ -0,0 +1,433 @@
+/*
+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 "io.h"
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Stereo.h>
+#include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMath.h>
+#include <opencv2/imgproc/types_c.h>
+#include <fstream>
+#include <string>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"evalStereo.exe left.png right.png calib.txt disp.pfm mask.png [Parameters]\n"
+			"Example (with http://vision.middlebury.edu/stereo datasets):\n"
+			"  $ ./rtabmap-stereoEval im0.png im1.png calib.txt disp0GT.pfm mask0nocc.png -Kp/DetectorStrategy 6 -Stereo/WinSize 5 -Stereo/MaxLevel 2 -Kp/WordsPerImage 1000 -Stereo/OpticalFlow false -Stereo/Iterations 5\n\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setLevel(ULogger::kDebug);
+	ULogger::setType(ULogger::kTypeConsole);
+
+	if(argc < 6)
+	{
+		showUsage();
+	}
+
+	ParametersMap parameters = Parameters::getDefaultParameters();
+	for(int i=6; i<argc; ++i)
+	{
+		// Check for RTAB-Map's parameters
+		std::string key = argv[i];
+		key = uSplit(key, '-').back();
+		if(parameters.find(key) != parameters.end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+				std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
+				if(inserted.second == false)
+				{
+					inserted.first->second = value;
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		//backward compatibility
+		// look for old parameter name
+		std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
+		if(oldIter!=Parameters::getRemovedParameters().end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+
+				if(oldIter->second.first)
+				{
+					key = oldIter->second.second;
+					UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
+							oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
+				}
+				else if(oldIter->second.second.empty())
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
+				}
+				else
+				{
+					UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
+				}
+				if(oldIter->second.first)
+				{
+					std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
+					if(inserted.second == false)
+					{
+						inserted.first->second = value;
+					}
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	UINFO("Loading files...");
+
+	cv::Mat left = cv::imread(argv[1]);
+	cv::Mat right = cv::imread(argv[2]);
+	cv::Mat disp = readPFM(argv[4]);
+	cv::Mat mask = cv::imread(argv[5]);
+
+	if(!left.empty() && !right.empty() && !disp.empty() && !mask.empty())
+	{
+		UASSERT(left.rows == disp.rows);
+		UASSERT(left.cols == disp.cols);
+		UASSERT(disp.rows == mask.rows);
+		UASSERT(disp.cols == mask.cols);
+
+		// read calib.txt
+		// Example format:
+		// --- calib.txt:
+		// cam0=[1038.018 0 322.037; 0 1038.018 243.393; 0 0 1]
+		// cam1=[1038.018 0 375.308; 0 1038.018 243.393; 0 0 1]
+		// doffs=53.271
+		// baseline=176.252
+		// width=718
+		// height=496
+		// ndisp=73
+		// isint=0
+		// vmin=8
+		// vmax=65
+		// dyavg=0.184
+		// dymax=0.423
+		// ---
+		std::string calibFile = argv[3];
+		std::ifstream stream(calibFile.c_str());
+		std::string line;
+
+		// two first lines are camera intrinsics
+		UINFO("Loading calibration... (%s)", calibFile.c_str());
+		std::vector<cv::Mat> K(2);
+		for(int i=0; i<2; ++i)
+		{
+			getline(stream, line);
+			line.erase(0, 6);
+			line = uReplaceChar(line, ']', "");
+			line = uReplaceChar(line, ';', "");
+			UINFO("K[%d] = %s", i, line.c_str());
+			std::vector<std::string> valuesStr = uListToVector(uSplit(line, ' '));
+			UASSERT(valuesStr.size() == 9);
+			K[i] = cv::Mat(3,3,CV_64FC1);
+			for(unsigned int j=0; j<valuesStr.size(); ++j)
+			{
+				K[i].at<double>(j) = uStr2Double(valuesStr[j]);
+			}
+		}
+
+		// skip doffs line
+		getline(stream, line);
+
+		// baseline
+		getline(stream, line);
+		line.erase(0, 9);
+		double baseline = uStr2Double(line);
+		UINFO("Baseline = %f", baseline);
+
+		StereoCameraModel model(
+				calibFile,
+				CameraModel(K[0].at<double>(0,0), K[0].at<double>(1,1), K[0].at<double>(0,2), K[0].at<double>(1,2)),
+				CameraModel(K[1].at<double>(0,0), K[1].at<double>(1,1), K[1].at<double>(0,2), K[1].at<double>(1,2), Transform::getIdentity(), -baseline/K[1].at<double>(0,0)));
+
+		UASSERT(model.isValidForProjection());
+
+		UINFO("Processing...");
+
+		// Processing...
+		cv::Mat leftMono;
+		if(left.channels() == 3)
+		{
+			cv::cvtColor(left, leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = left;
+		}
+		cv::Mat rightMono;
+		if(right.channels() == 3)
+		{
+			cv::cvtColor(right, rightMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			rightMono = right;
+		}
+
+		UTimer timer;
+		double timeKpts;
+		double timeSubPixel;
+		double timeStereo;
+
+		// generate kpts
+		std::vector<cv::KeyPoint> kpts;
+		uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), "0.03 0.03 0.04 0.04"));
+		Feature2D * kptDetector = Feature2D::create(parameters);
+		kpts = kptDetector->generateKeypoints(leftMono);
+		delete kptDetector;
+
+		timeKpts = timer.ticks();
+
+		std::vector<cv::Point2f> leftCorners(kpts.size());
+		cv::KeyPoint::convert(kpts, leftCorners);
+		int subPixWinSize = 0;
+		int subPixIterations = 0;
+		double subPixEps = 0;
+		Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), subPixWinSize);
+		Parameters::parse(parameters, Parameters::kKpSubPixIterations(), subPixIterations);
+		Parameters::parse(parameters, Parameters::kKpSubPixEps(), subPixEps);
+		if(subPixWinSize > 0 && subPixIterations > 0)
+		{
+			UDEBUG("cv::cornerSubPix() begin");
+			cv::cornerSubPix(leftMono, leftCorners,
+				cv::Size( subPixWinSize, subPixWinSize ),
+				cv::Size( -1, -1 ),
+				cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
+			UDEBUG("cv::cornerSubPix() end");
+		}
+
+		timeSubPixel = timer.ticks();
+
+		// Find features in the new right image
+		std::vector<unsigned char> status;
+		std::vector<cv::Point2f> rightCorners;
+
+		bool opticalFlow = false;
+		Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
+		Stereo * stereo = 0;
+		if(opticalFlow)
+		{
+			stereo = new StereoOpticalFlow(parameters);
+		}
+		else
+		{
+			stereo = new Stereo(parameters);
+		}
+
+		rightCorners = stereo->computeCorrespondences(
+				leftMono,
+				rightMono,
+				leftCorners,
+				status);
+		delete stereo;
+
+		timeStereo = timer.ticks();
+
+		UINFO("Time: kpts:%f s, subpix=%f s, stereo=%f s", timeKpts, timeSubPixel, timeStereo);
+
+		UDEBUG("Mask = %d", mask.type());
+
+		int inliers = 0;
+		int subInliers = 0;
+		int badInliers = 0;
+		float sumInliers = 0.0f;
+		float sumSubInliers = 0.0f;
+		int goodRejected = 0;
+		int badRejected = 0;
+		for(unsigned int i=0; i<leftCorners.size(); ++i)
+		{
+			float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
+			if(status[i]!=0)
+			{
+				float d = leftCorners[i].x - rightCorners[i].x;
+				//float err = fabs(d-gt);
+				//UDEBUG("Pt(%f,%f): d=%f, gt=%f, error=%f", leftCorners[i].x, leftCorners[i].y, d, gt, err);
+
+				if(uIsFinite(gt))
+				{
+					if(fabs(d-gt) < 1.0f)
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 0, 255, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+
+						++inliers;
+						sumInliers += fabs(d-gt);
+
+						if(fabs(d-gt) < 0.5f)
+						{
+							++subInliers;
+							sumSubInliers += fabs(d-gt);
+						}
+					}
+					else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 0, 255, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+						++badInliers;
+						//UDEBUG("should be rejected or refined: %d pt=(%f,%f) (d=%f gt=%f)", i, leftCorners[i].x, leftCorners[i].y, d, gt);
+					}
+					else
+					{
+						cv::line(left,
+							leftCorners[i],
+							cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+							cv::Scalar( 255, 0, 0 ));
+
+						cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+							cv::Scalar( 0, 0, 255 ));
+						++badInliers;
+						//UDEBUG("should be rejected: %d", i);
+					}
+				}
+				else
+				{
+					++badInliers;
+				}
+			}
+			else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
+					rightCorners[i].x > 0.0f)
+			{
+				float d = leftCorners[i].x - rightCorners[i].x;
+				if(fabs(d-gt) < 1.0f)
+				{
+					cv::line(left,
+						leftCorners[i],
+						cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
+						cv::Scalar( 0, 255, 255 ));
+
+					cv::line(left,
+							cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
+							cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
+						cv::Scalar( 0, 0, 255 ));
+					++goodRejected;
+					//UDEBUG("should not be rejected: %d", i);
+				}
+				else
+				{
+					++badRejected;
+				}
+			}
+			else
+			{
+				++badRejected;
+				//UDEBUG("correctly rejected: %d", i);
+			}
+		}
+
+		UINFO("good accepted=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
+				inliers,
+				(inliers*100)/leftCorners.size(),
+				badInliers,
+				(badInliers*100)/leftCorners.size(),
+				goodRejected,
+				(goodRejected*100)/leftCorners.size(),
+				badRejected,
+				(badRejected*100)/leftCorners.size());
+		UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
+
+
+		cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Right", right );
+
+		cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Mask", mask );
+
+		cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
+		cv::imshow( "Left", left );
+
+		cv::waitKey(0);
+
+	}
+
+	return 0;
+}

rtabmap/tools/VocabularyComparison/CMakeLists.txt

@@ -0,0 +1,7 @@
+
+ADD_EXECUTABLE(vocabularyComparison main.cpp)
+TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_core)
+
+SET_TARGET_PROPERTIES( vocabularyComparison 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-vocabularyComparison)
+

rtabmap/tools/VocabularyComparison/main.cpp

@@ -0,0 +1,289 @@
+/*
+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 <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/flann/miniflann.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/nonfree/features2d.hpp>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <fstream>
+#include <vector>
+#include <list>
+#include <string>
+#include <iostream>
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"vocabularyComparison.exe \"dictionary/path\"\n"
+			"  Dictionary path example: \"data/Dictionary49k.txt\""
+			"  Note that 400 first descriptors in the file are used as queries.\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kDebug);
+
+	std::string dictionaryPath = argv[argc-1];
+	std::list<std::vector<float> > objectDescriptors;
+	//std::list<std::vector<float> > descriptors;
+	std::map<int, std::vector<float> > descriptors;
+	int dimension  = 0;
+	UTimer timer;
+	int objectDescriptorsSize= 400;
+
+	std::ifstream file;
+	if(!dictionaryPath.empty())
+	{
+		file.open(dictionaryPath.c_str(), std::ifstream::in);
+	}
+	if(file.good())
+	{
+		UDEBUG("Loading the dictionary from \"%s\"", dictionaryPath.c_str());
+
+		// first line is the header
+		std::string str;
+		std::list<std::string> strList;
+		std::getline(file, str);
+		strList = uSplitNumChar(str);
+		for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
+		{
+			if(uIsDigit(iter->at(0)))
+			{
+				dimension = std::atoi(iter->c_str());
+				break;
+			}
+		}
+
+		if(dimension == 0 || dimension > 1000)
+		{
+			UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
+		}
+		else
+		{
+			int descriptorsLoaded = 0;
+			// Process all words
+			while(file.good())
+			{
+				std::getline(file, str);
+				strList = uSplit(str);
+				if((int)strList.size() == dimension+1)
+				{
+					//first one is the visual word id
+					std::list<std::string>::iterator iter = strList.begin();
+					int id = atoi(iter->c_str());
+					++iter;
+
+					std::vector<float> descriptor(dimension);
+					int i=0;
+
+					//get descriptor
+					for(;i<dimension && iter != strList.end(); ++i, ++iter)
+					{
+						descriptor[i] = uStr2Float(*iter);
+					}
+					if(i != dimension)
+					{
+						UERROR("");
+					}
+
+					if(++descriptorsLoaded<=objectDescriptorsSize)
+					{
+						objectDescriptors.push_back(descriptor);
+					}
+					else
+					{
+						//descriptors.push_back(descriptor);
+						descriptors.insert(std::make_pair(id, descriptor));
+					}
+				}
+				else if(str.size())
+				{
+					UWARN("Cannot parse line \"%s\"", str.c_str());
+				}
+			}
+		}
+
+		UDEBUG("Time loading dictionary = %fs, dimension=%d", timer.ticks(), dimension);
+	}
+	else
+	{
+		UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
+	}
+	file.close();
+
+	if(descriptors.size() && objectDescriptors.size() && dimension)
+	{
+		cv::Mat dataTree;
+		cv::Mat queries;
+
+		UDEBUG("Creating data structures...");
+		// Create the data structure
+		dataTree = cv::Mat((int)descriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
+		{//scope
+			//std::list<std::vector<float> >::const_iterator iter = descriptors.begin();
+			std::map<int, std::vector<float> >::const_iterator iter = descriptors.begin();
+			for(unsigned int i=0; i < descriptors.size(); ++i, ++iter)
+			{
+				UTimer tim;
+				//memcpy(dataTree.ptr<float>(i), iter->data(), dimension*sizeof(float));
+				memcpy(dataTree.ptr<float>(i), iter->second.data(), dimension*sizeof(float));
+				//if(i%100==0)
+				//	UDEBUG("i=%d/%d tim=%fs", i, descriptors.size(), tim.ticks());
+			}
+		}
+
+		queries = cv::Mat((int)objectDescriptors.size(), dimension, CV_32F); // SURF descriptors are CV_32F
+		{//scope
+			std::list<std::vector<float> >::const_iterator iter = objectDescriptors.begin();
+			for(unsigned int i=0; i < objectDescriptors.size(); ++i, ++iter)
+			{
+				UTimer tim;
+				memcpy(queries.ptr<float>(i), iter->data(), dimension*sizeof(float));
+				//if(i%100==0)
+				//	UDEBUG("i=%d/%d tim=%fs", i, objectDescriptors.size(), tim.ticks());
+			}
+		}
+
+		UDEBUG("descriptors.size()=%d, objectDescriptorsSize=%d, copying data = %f s",descriptors.size(), objectDescriptors.size(), timer.ticks());
+
+		UDEBUG("Creating indexes...");
+		cv::flann::Index * linearIndex = new cv::flann::Index(dataTree, cv::flann::LinearIndexParams());
+		UDEBUG("Time to create linearIndex = %f s", timer.ticks());
+
+		cv::flann::Index * kdTreeIndex1 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(1));
+		UDEBUG("Time to create kdTreeIndex1 = %f s", timer.ticks());
+
+		cv::flann::Index * kdTreeIndex4 = new cv::flann::Index(dataTree, cv::flann::KDTreeIndexParams(4));
+		UDEBUG("Time to create kdTreeIndex4 = %f s", timer.ticks());
+
+		cv::flann::Index * kMeansIndex = new cv::flann::Index(dataTree, cv::flann::KMeansIndexParams());
+		UDEBUG("Time to create kMeansIndex = %f s", timer.ticks());
+
+		cv::flann::Index * compositeIndex = new cv::flann::Index(dataTree, cv::flann::CompositeIndexParams());
+		UDEBUG("Time to create compositeIndex = %f s", timer.ticks());
+
+		//cv::flann::Index * autoTunedIndex = new cv::flann::Index(dataTree, cv::flann::AutotunedIndexParams());
+		//UDEBUG("Time to create autoTunedIndex = %f s", timer.ticks());
+
+
+		UDEBUG("Search indexes...");
+		int k=2; // 2 nearest neighbors
+		cv::Mat results(queries.rows, k, CV_32SC1); // results index
+		cv::Mat dists(queries.rows, k, CV_32FC1); // Distance results are CV_32FC1
+
+		linearIndex->knnSearch(queries, results, dists, k);
+		//std::cout << results.t() << std::endl;
+		cv::Mat transposedLinear = dists.t();
+		UDEBUG("Time to search linearIndex = %f s", timer.ticks());
+
+		kdTreeIndex1->knnSearch(queries, results, dists, k);
+		//std::cout << results.t() << std::endl;
+		cv::Mat transposed = dists.t();
+		UDEBUG("Time to search kdTreeIndex1 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
+				timer.ticks(),
+				transposed.cols,
+				uMeanSquaredError(  (float*)transposed.data,
+					transposed.cols,
+					(float*)transposedLinear.data,
+					transposedLinear.cols),
+				uMeanSquaredError(  &transposed.at<float>(1,0),
+					transposed.cols,
+					&transposedLinear.at<float>(1,0),
+					transposedLinear.cols));
+
+		kdTreeIndex4->knnSearch(queries, results, dists, k);
+		//std::cout << results.t() << std::endl;
+		transposed = dists.t();
+		UDEBUG("Time to search kdTreeIndex4 = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
+				timer.ticks(),
+				transposed.cols,
+				uMeanSquaredError(  (float*)transposed.data,
+					transposed.cols,
+					(float*)transposedLinear.data,
+					transposedLinear.cols),
+				uMeanSquaredError(  &transposed.at<float>(1,0),
+					transposed.cols,
+					&transposedLinear.at<float>(1,0),
+					transposedLinear.cols));
+
+		kMeansIndex->knnSearch(queries, results, dists, k);
+		//std::cout << results.t() << std::endl;
+		transposed = dists.t();
+		UDEBUG("Time to search kMeansIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
+				timer.ticks(),
+				transposed.cols,
+				uMeanSquaredError(  (float*)transposed.data,
+					transposed.cols,
+					(float*)transposedLinear.data,
+					transposedLinear.cols),
+				uMeanSquaredError(  &transposed.at<float>(1,0),
+					transposed.cols,
+					&transposedLinear.at<float>(1,0),
+					transposedLinear.cols));
+
+		compositeIndex->knnSearch(queries, results, dists, k);
+		//std::cout << results.t() << std::endl;
+		transposed = dists.t();
+		UDEBUG("Time to search compositeIndex = %f s (size=%d, dist error(k1,k2)=(%f,%f))",
+				timer.ticks(),
+				transposed.cols,
+				uMeanSquaredError(  (float*)transposed.data,
+					transposed.cols,
+					(float*)transposedLinear.data,
+					transposedLinear.cols),
+				uMeanSquaredError(  &transposed.at<float>(1,0),
+					transposed.cols,
+					&transposedLinear.at<float>(1,0),
+					transposedLinear.cols));
+
+		//autoTunedIndex->knnSearch(queries, results, dists, k);
+		//UDEBUG("Time to search autoTunedIndex = %f s", timer.ticks());
+
+		delete linearIndex;
+		delete kdTreeIndex1;
+		delete kdTreeIndex4;
+		delete kMeansIndex;
+		delete compositeIndex;
+		//delete autoTunedIndex;
+	}
+
+    return 0;
+}