Merged Audio branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83

tools/CMakeLists.txt

@@ -0,0 +1,14 @@
+
+ADD_SUBDIRECTORY( ConsoleApp )
+ADD_SUBDIRECTORY( ImagesJoiner )
+ADD_SUBDIRECTORY( WebcamCapture )
+ADD_SUBDIRECTORY( Polar )
+ADD_SUBDIRECTORY( ImagesDbExtractor )
+ADD_SUBDIRECTORY( ColorIndexesGenerator )
+
+IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+	ADD_SUBDIRECTORY( DatabaseViewer )
+	ADD_SUBDIRECTORY( EpipolarGeometry )
+ELSE()
+   MESSAGE(STATUS "[WARNING] Qt4 not found, the databaseViewer and epipolarGeometry programs will not be built...")
+ENDIF()

tools/ColorIndexesGenerator/CMakeLists.txt

@@ -0,0 +1,32 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+	${ZLIB_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${ZLIB_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(colorIndexesGenerator ${SRC_FILES})
+TARGET_LINK_LIBRARIES(colorIndexesGenerator rtabmap_corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( colorIndexesGenerator 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-colorIndexesGenerator)
+
+INSTALL(TARGETS colorIndexesGenerator
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/ColorIndexesGenerator/main.cpp

@@ -0,0 +1,228 @@
+#include <opencv2/core/core.hpp>
+#include <iostream>
+#include <fstream>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include "rtabmap/core/ColorTable.h"
+#include "rtabmap/core/NearestNeighbor.h"
+#include <zlib.h>
+#include <utilite/UConversion.h>
+
+#define FILE_NAME_PREFIX "ColorIndexes"
+#define FILE_NAME_SUFFIX ".bin"
+#define CHUNK 16384
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-colorIndexesGenerator size\n"
+			"  size          Supported : 8, 16, 32, 64, 128, 256, 512, 1024, 65536\n");
+	exit(1);
+}
+
+cv::Mat generateColorTable()
+{
+	cv::Mat colorTable(256*256*256, 3, CV_32F);
+	for(int b=0; b<256; ++b)
+	{
+		for(int g=0; g<256; ++g)
+		{
+			for(int r=0; r<256; ++r)
+			{
+				colorTable.at<float>(b*256*256 + g*256 + r, 0) = (float)r;
+				colorTable.at<float>(b*256*256 + g*256 + r, 1) = (float)g;
+				colorTable.at<float>(b*256*256 + g*256 + r, 2) = (float)b;
+				UDEBUG("r=%f, g=%f, b=%f", (float)r , (float)g, (float)b);
+			}
+		}
+	}
+	return colorTable;
+}
+
+/* report a zlib or i/o error */
+void zerr(int ret)
+{
+    fputs("zpipe: ", stderr);
+    switch (ret) {
+    case Z_ERRNO:
+        if (ferror(stdin))
+            fputs("error reading stdin\n", stderr);
+        if (ferror(stdout))
+            fputs("error writing stdout\n", stderr);
+        break;
+    case Z_STREAM_ERROR:
+        fputs("invalid compression level\n", stderr);
+        break;
+    case Z_DATA_ERROR:
+        fputs("invalid or incomplete deflate data\n", stderr);
+        break;
+    case Z_MEM_ERROR:
+        fputs("out of memory\n", stderr);
+        break;
+    case Z_VERSION_ERROR:
+        fputs("zlib version mismatch!\n", stderr);
+        break;
+    }
+    UFATAL("");
+}
+
+/* Compress from file source to file dest until EOF on source.
+   def() returns Z_OK on success, Z_MEM_ERROR if memory could not be
+   allocated for processing, Z_STREAM_ERROR if an invalid compression
+   level is supplied, Z_VERSION_ERROR if the version of zlib.h and the
+   version of the library linked do not match, or Z_ERRNO if there is
+   an error reading or writing the files. */
+int def(FILE *source, FILE *dest, int level)
+{
+    int ret, flush;
+    unsigned have;
+    z_stream strm;
+    unsigned char in[CHUNK];
+    unsigned char out[CHUNK];
+
+    /* allocate deflate state */
+    strm.zalloc = Z_NULL;
+    strm.zfree = Z_NULL;
+    strm.opaque = Z_NULL;
+    ret = deflateInit(&strm, level);
+    if (ret != Z_OK)
+        return ret;
+    UDEBUG("");
+    /* compress until end of file */
+    do {
+    	UDEBUG("");
+        strm.avail_in = fread(in, 1, CHUNK, source);
+        if (ferror(source)) {
+            (void)deflateEnd(&strm);
+            return Z_ERRNO;
+        }
+        flush = feof(source) ? Z_FINISH : Z_NO_FLUSH;
+        strm.next_in = in;
+
+        /* run deflate() on input until output buffer not full, finish
+           compression if all of source has been read in */
+        do {
+        	UDEBUG("");
+            strm.avail_out = CHUNK;
+            strm.next_out = out;
+            ret = deflate(&strm, flush);    /* no bad return value */
+            assert(ret != Z_STREAM_ERROR);  /* state not clobbered */
+            have = CHUNK - strm.avail_out;
+            if (fwrite(out, 1, have, dest) != have || ferror(dest)) {
+                (void)deflateEnd(&strm);
+                return Z_ERRNO;
+            }
+        } while (strm.avail_out == 0);
+        assert(strm.avail_in == 0);     /* all input will be used */
+
+        /* done when last data in file processed */
+    } while (flush != Z_FINISH);
+    assert(ret == Z_STREAM_END);        /* stream will be complete */
+
+    /* clean up and return */
+    (void)deflateEnd(&strm);
+    return Z_OK;
+}
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setPrintWhere(false);
+
+	UTimer timer;
+
+	if(argc<2)
+	{
+		showUsage();
+	}
+	int size = atoi(argv[1]);
+
+	rtabmap::FlannKdTreeNN nn;
+	cv::Mat colorTable;
+	switch(size)
+	{
+	case 8:
+		colorTable = cv::Mat(8, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_8);
+		break;
+	case 16:
+		colorTable = cv::Mat(16, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_16);
+		break;
+	case 32:
+		colorTable = cv::Mat(32, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_32);
+		break;
+	case 64:
+		colorTable = cv::Mat(64, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_64);
+		break;
+	case 128:
+		colorTable = cv::Mat(128, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_128);
+		break;
+	case 256:
+		colorTable = cv::Mat(256, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_256);
+		break;
+	case 512:
+		colorTable = cv::Mat(512, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_512);
+		break;
+	case 1024:
+		colorTable = cv::Mat(1024, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_1024);
+		break;
+	case 65536:
+		colorTable = cv::Mat(65536, 3, CV_8U, rtabmap::ColorTable::INDEXED_TABLE_65536);
+		break;
+	default:
+		printf("\nSize not supported...\n");
+		showUsage();
+		break;
+	}
+
+	cv::Mat data;
+	colorTable.convertTo(data, CV_32F);
+
+	if(data.rows > 0x10000)
+	{
+		UFATAL("Color table is too big (%d > 2^16)", data.rows);
+	}
+
+	UINFO("Generating full color cube...");
+	cv::Mat queries = generateColorTable();
+	UINFO("Generating full color cube... done!");
+
+	cv::Mat dists(queries.rows, 1, CV_32F);
+	cv::Mat indices = cv::Mat(queries.rows, 1, CV_32S);
+
+	UINFO("Nearest neighbor searching (queries=%d, indexes=%d)...", queries.rows, data.rows);
+	nn.search(data, queries, indices, dists);
+	UINFO("Nearest neighbor searching (queries=%d, indexes=%d)... done!", queries.rows, data.rows);
+
+	std::string fileName = uFormat("%s%d%s", FILE_NAME_PREFIX, size, FILE_NAME_SUFFIX);
+	UINFO("Saving color indexed table to %s...", fileName.c_str());
+	std::ofstream outfile(fileName.c_str(), std::ios_base::out | std::ios_base::binary);
+	unsigned short index;
+	for(int i=0; i<indices.rows; ++i)
+	{
+		UDEBUG("%d = %d", i, indices.at<int>(i,0));
+		index = (unsigned short)indices.at<int>(i,0); // assume indexes are under 2^16
+		outfile.write((const char *)&index, sizeof(unsigned short));
+	}
+	outfile.close();
+	UINFO("Saving color indexed table to %s... done!", fileName.c_str());
+
+	std::string zipFileName = fileName + std::string(".zip");
+	UINFO("Compressing file %s to %s...", fileName.c_str(), zipFileName.c_str());
+	FILE * input = fopen(fileName.c_str(), "rb");
+	FILE * compressed = fopen(zipFileName.c_str(), "wb");
+
+	int result = def(input, compressed, Z_DEFAULT_COMPRESSION);
+	if(result != Z_OK)
+	{
+		zerr(result);
+		UERROR("");
+	}
+
+	fclose(input);
+	fclose(compressed);
+	UINFO("Compressing file %s to %s... done!", fileName.c_str(), zipFileName.c_str());
+
+	UINFO("Total time = %f s", timer.getElapsedTime());
+    return 0;
+}

tools/ConsoleApp/CMakeLists.txt

@@ -0,0 +1,34 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBRARIES} 
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+# Add binary called "consoleApp" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(consoleApp ${SRC_FILES})
+TARGET_LINK_LIBRARIES(consoleApp rtabmap_corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( consoleApp 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console)
+
+INSTALL(TARGETS consoleApp
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/ConsoleApp/main.cpp

@@ -0,0 +1,545 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/Camera.h"
+#include <utilite/UDirectory.h>
+#include <utilite/UFile.h>
+#include <utilite/UConversion.h>
+#include <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"
+			"  -t #.##                         Time threshold (ms)\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"
+			"  -image_width #                  Force an image width (Default 0: original size used).\n"
+			"                                   The height must be also specified if changed.\n"
+			"  -image_height #                 Force an image height (Default 0: original size used)\n"
+			"                                   The height must be also specified if changed.\n"
+			"  -frames_dropped #               Frames dropped from source (Default 0: no frames dropped)\n"
+			"  -start_at #                     When \"path\" is a directory of images, set this parameter\n"
+			"                                   to start processing at image # (default 1)."
+			"  -\"parameter name\" \"value\"       Overwrite a specific RTAB-Map's parameter :\n"
+			"                                     -SURF/HessianThreshold 150\n"
+			"                                   For parameters in table format, add ',' between values :\n"
+			"                                     -Kp/RoiRatios 0,0,0.1,0\n"
+			"                                   Default parameters can be found in ~/.rtabmap/rtabmap.ini\n"
+			"  -default_params                 Show default RTAB-Map's parameters (WARNING : \n"
+			"                                   parameters from rtabmap.ini (if exists) overwrite the default \n"
+			"                                   ones shown here)\n"
+			"  -debug                          Set Log level to Debug (Default Error)\n"
+			"  -info                           Set Log level to Info (Default Error)\n"
+			"  -warn                           Set Log level to Warning (Default Error)\n"
+			"  -exit_warn                      Set exit level to Warning (Default Fatal)\n"
+			"  -exit_error                     Set exit level to Error (Default Fatal)\n"
+			"  -v                              Get version of RTAB-Map\n");
+	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);
+
+	/*for(int i=0; i<argc; i++)
+	{
+		printf("argv[%d] = %s\n", i, argv[i]);
+	}*/
+	const ParametersMap & defaultParameters = Parameters::getDefaultParameters();
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else if(argc == 2 && strcmp(argv[1], "-v") == 0)
+	{
+		printf("%s\n", Rtabmap::getVersion().c_str());
+		exit(0);
+	}
+	else if(argc == 2 && strcmp(argv[1], "-default_params") == 0)
+	{
+		for(ParametersMap::const_iterator iter = defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
+		{
+			printf("%s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+		exit(0);
+	}
+	printf("\n");
+
+	std::string path;
+	float timeThreshold = 0.0;
+	float rate = 0.0;
+	int loopDataset = 0;
+	int repeat = 0;
+	bool createGT = false;
+	int imageWidth = 0;
+	int imageHeight = 0;
+	int startAt = 1;
+	int framesDropped = 0;
+	ParametersMap pm;
+	ULogger::Level logLevel = ULogger::kError;
+	ULogger::Level exitLevel = ULogger::kFatal;
+
+	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], "-t") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				timeThreshold = std::atof(argv[i]);
+				if(timeThreshold < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-rate") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = std::atof(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-rateHz") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = std::atof(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], "-image_width") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				imageWidth = std::atoi(argv[i]);
+				if(imageWidth < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-image_height") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				imageHeight = std::atoi(argv[i]);
+				if(imageHeight < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-frames_dropped") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				framesDropped = std::atoi(argv[i]);
+				if(framesDropped < 0)
+				{
+					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], "-createGT") == 0)
+		{
+			createGT = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-debug") == 0)
+		{
+			logLevel = ULogger::kDebug;
+			continue;
+		}
+		if(strcmp(argv[i], "-info") == 0)
+		{
+			logLevel = ULogger::kInfo;
+			continue;
+		}
+		if(strcmp(argv[i], "-warn") == 0)
+		{
+			logLevel = ULogger::kWarning;
+			continue;
+		}
+		if(strcmp(argv[i], "-exit_warn") == 0)
+		{
+			exitLevel = ULogger::kWarning;
+			continue;
+		}
+		if(strcmp(argv[i], "-exit_error") == 0)
+		{
+			exitLevel = ULogger::kError;
+			continue;
+		}
+
+		// Check for RTAB-Map's parameters
+		std::string key = argv[i];
+		key = uSplit(key, '-').back();
+		if(defaultParameters.find(key) != defaultParameters.end())
+		{
+			++i;
+			if(i < argc)
+			{
+				std::string value = argv[i];
+				if(value.empty())
+				{
+					showUsage();
+				}
+				else
+				{
+					value = uReplaceChar(value, ',', ' ');
+				}
+				std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
+				if(inserted.second == false)
+				{
+					inserted.first->second = value;
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		printf("Unrecognized option : %s\n", argv[i]);
+		showUsage();
+	}
+
+	if(repeat && createGT)
+	{
+		printf("Cannot create a Ground truth if repeat is on.\n");
+		showUsage();
+	}
+	else if((imageWidth && imageHeight == 0) ||
+			(imageHeight && imageWidth == 0))
+	{
+		printf("If imageWidth is set, imageHeight must be too.\n");
+		showUsage();
+	}
+
+	UTimer timer;
+	timer.start();
+	std::queue<double> iterationMeanTime;
+
+	Camera * camera = 0;
+	if(UDirectory::exists(path))
+	{
+		camera = new CameraImages(path, startAt, false, 1/rate, false, imageWidth, imageHeight, framesDropped);
+	}
+	else
+	{
+		camera = new CameraVideo(path, 1/rate, false, imageWidth, imageHeight, framesDropped);
+	}
+
+	if(!camera || !camera->init())
+	{
+		printf("Camera init failed, using path \"%s\"\n", path.c_str());
+		exit(1);
+	}
+
+	std::map<int, int> groundTruth;
+
+	// Create tasks
+	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->init();
+	rtabmap->setMaxTimeAllowed(timeThreshold); // in ms
+
+	//ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setType(ULogger::kTypeFile, rtabmap->getWorkingDir()+"/LogConsole.txt", false);
+	ULogger::setBuffered(true);
+	ULogger::setLevel(logLevel);
+	ULogger::setExitLevel(exitLevel);
+
+	// Disable statistics (we don't need them)
+	pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), uBool2Str(false)));
+	rtabmap->init(pm);
+
+	printf("Avpd 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", timeThreshold);
+	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
+	printf(" Repeating data set = %s\n", repeat?"true":"false");
+	printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
+	printf(" Camera starts at image %d (default 1)\n", startAt);
+	printf(" Camera frames dropped %d (default 0)\n", framesDropped);
+	if(createGT)
+	{
+		printf(" Creating the ground truth matrix.\n");
+	}
+	printf(" INFO: All other parameters are taken from the INI file located in \"~/.rtabmap\"\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->getWorkingMem().size() || rtabmap->getStMem().size())
+	{
+		printf("[Warning] RTAB-Map database is not empty (%s)\n", (rtabmap->getWorkingDir()+Rtabmap::kDefaultDatabaseName).c_str());
+	}
+	printf("\nProcessing images...\n");
+
+	//setup camera
+	ParametersMap allParam;
+	Rtabmap::readParameters(rtabmap->getIniFilePath().c_str(), allParam);
+	pm.insert(allParam.begin(), allParam.end());
+	camera->setFeaturesExtracted(true);
+	camera->parseParameters(pm);
+
+	UTimer iterationTimer;
+	int imagesProcessed = 0;
+	std::list<std::vector<float> > teleopActions;
+	while(loopDataset <= repeat && g_forever)
+	{
+		cv::Mat descriptors;
+		std::vector<cv::KeyPoint> keypoints;
+		cv::Mat img = camera->takeImage(descriptors, keypoints);
+		int i=0;
+		while(!img.empty() && g_forever)
+		{
+			++imagesProcessed;
+			iterationTimer.start();
+			rtabmap->process(Sensor(descriptors, keypoints));
+			loopClosureId = rtabmap->getLoopClosureId();
+			if(rtabmap->getLoopClosureId())
+			{
+				++countLoopDetected;
+			}
+			img = camera->takeImage(descriptors, keypoints);
+			if(++count % 100 == 0)
+			{
+				printf(" count = %d, loop closures = %d\n", count, countLoopDetected);
+				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)
+				{
+					groundTruth.insert(std::make_pair(i, loopClosureId-1));
+				}
+			}
+
+			++i;
+
+			double iterationTime = iterationTimer.ticks();
+
+			ULogger::flush();
+
+			if(rtabmap->getLoopClosureId())
+			{
+				printf(" iteration(%d) loop(%d) time=%fs *\n", count, rtabmap->getLoopClosureId(), iterationTime);
+			}
+			else if(rtabmap->getReactivatedId())
+			{
+				printf(" iteration(%d) high(%d) time=%fs\n", count, rtabmap->getReactivatedId(), iterationTime);
+			}
+			else
+			{
+				printf(" iteration(%d) time=%fs\n", count, iterationTime);
+			}
+
+			if(timeThreshold && iterationTime > timeThreshold*100.0f)
+			{
+				printf(" ERROR,  there is  problem, too much time taken... %fs", iterationTime);
+				break; // there is  problem, don't continue
+			}
+		}
+		++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(imagesProcessed && createGT)
+	{
+		cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
+
+		for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter)
+		{
+			groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
+		}
+
+		// Generate the ground truth file
+		printf("Generate ground truth to file %s, size of %d\n", (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), groundTruthMat.rows);
+		IplImage img = groundTruthMat;
+		cvSaveImage((rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), &img);
+		printf(" Creating ground truth file = %fs\n", timer.ticks());
+	}
+
+	if(camera)
+	{
+		delete camera;
+		camera = 0 ;
+	}
+
+	if(rtabmap)
+	{
+		delete rtabmap;
+		rtabmap = 0;
+	}
+
+	printf(" Cleanup time = %fs\n", timer.ticks());
+
+	return 0;
+}

tools/DatabaseViewer/CMakeLists.txt

@@ -0,0 +1,56 @@
+### Qt Gui stuff ###
+SET(headers_ui 
+	./MainWindow.h
+)
+
+SET(uis
+    ./ui/MainWindow.ui
+)
+
+#Generate .h files from the .ui files
+QT4_WRAP_UI(moc_uis ${uis})
+
+#This will generate moc_* for Qt
+QT4_WRAP_CPP(moc_srcs ${headers_ui})
+### Qt Gui stuff  end###
+
+
+SET(SRC_FILES 
+	./main.cpp
+	./MainWindow.cpp
+	${moc_srcs} 
+	${moc_uis} 
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+	${UTILITE_INCLUDE_DIRS}
+	${OpenCV_INCLUDE_DIRS}
+	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
+)
+
+INCLUDE(${QT_USE_FILE})
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+    ${QT_LIBRARIES} 
+	${OpenCV_LIBRARIES} 
+	#${QWT5_LIBRARY} 
+)
+
+#include files
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+# Add binary called "databaseViewer" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(databaseViewer WIN32 ${SRC_FILES})
+TARGET_LINK_LIBRARIES(databaseViewer rtabmap_corelib rtabmap_guilib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( databaseViewer 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer)
+
+INSTALL(TARGETS databaseViewer
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)

tools/DatabaseViewer/MainWindow.cpp

@@ -0,0 +1,552 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "MainWindow.h"
+#include "ui_MainWindow.h"
+#include <QtGui/QMessageBox>
+#include <QtGui/QFileDialog>
+#include <QtGui/QInputDialog>
+#include <QtCore/QBuffer>
+#include <QtCore/QTextStream>
+#include <utilite/ULogger.h>
+#include <utilite/UDirectory.h>
+#include <utilite/UConversion.h>
+#include <opencv2/core/core_c.h>
+#include <utilite/UTimer.h>
+#include "rtabmap/core/KeypointMemory.h"
+#include "rtabmap/core/SMMemory.h"
+#include "rtabmap/core/DBDriver.h"
+#include "rtabmap/gui/KeypointItem.h"
+
+MainWindow::MainWindow(QWidget * parent) :
+	QMainWindow(parent),
+	memory_(0)
+{
+	pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
+
+	if(!UDirectory::exists(pathDatabase_.toStdString()))
+	{
+		pathDatabase_ = QDir::homePath();
+	}
+
+	ui_ = new Ui_MainWindow();
+	ui_->setupUi(this);
+
+	connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
+
+	// connect actions with custom slots
+	connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
+	connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
+	connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
+	connect(ui_->actionClean_database, SIGNAL(triggered()), this, SLOT(cleanDatabase()));
+	connect(ui_->actionClean_local_graph, SIGNAL(triggered()), this, SLOT(cleanLocalGraph()));
+
+	ui_->graphicsView_A->setScene(new QGraphicsScene(this));
+	ui_->graphicsView_B->setScene(new QGraphicsScene(this));
+
+	ui_->horizontalSlider_A->setTracking(false);
+	ui_->horizontalSlider_B->setTracking(false);
+	ui_->horizontalSlider_A->setEnabled(false);
+	ui_->horizontalSlider_B->setEnabled(false);
+	connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
+	connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
+	connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
+	connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
+}
+
+MainWindow::~MainWindow()
+{
+	delete ui_;
+	if(memory_)
+	{
+		delete memory_;
+	}
+}
+
+void MainWindow::openDatabase()
+{
+	QStringList types;
+	types << "Keypoint" << "Sensorimotor";
+	bool ok = false;
+	QString type = QInputDialog::getItem(this, tr("Select database type"), tr("Type"), types, 0, false, &ok);
+	if(ok && !type.isEmpty())
+	{
+		QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
+		if(!path.isEmpty())
+		{
+			if(memory_)
+			{
+				delete memory_;
+				memory_ = 0;
+				imagesMap_.clear();
+				ids_.clear();
+			}
+
+			std::string driverType = "sqlite3";
+			rtabmap::ParametersMap parameters;
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
+			if(type.compare("Keypoint") == 0)
+			{
+				memory_ = new rtabmap::KeypointMemory(parameters);
+			}
+			else
+			{
+				memory_ = new rtabmap::SMMemory(parameters);
+			}
+			if(!memory_)
+			{
+				QMessageBox::warning(this, "Database error", tr("Can't create database driver \"%1\"").arg(driverType.c_str()));
+			}
+			else if(!memory_->init(driverType, path.toStdString()))
+			{
+				QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
+			}
+			else
+			{
+				pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
+				updateIds();
+			}
+		}
+	}
+
+
+}
+
+void MainWindow::updateIds()
+{
+	if(!memory_)
+	{
+		return;
+	}
+
+	std::set<int> ids = memory_->getAllSignatureIds();
+	ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
+	ids_.prepend(0);
+
+	UDEBUG("Loaded %d ids", ids_.size());
+
+	if(ids_.size())
+	{
+		ui_->horizontalSlider_A->setMinimum(0);
+		ui_->horizontalSlider_B->setMinimum(0);
+		ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
+		ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
+		ui_->horizontalSlider_A->setSliderPosition(0);
+		ui_->horizontalSlider_B->setSliderPosition(0);
+		ui_->horizontalSlider_A->setEnabled(true);
+		ui_->horizontalSlider_B->setEnabled(true);
+		ui_->label_idA->setText("0");
+		ui_->label_idB->setText("0");
+	}
+	else
+	{
+		ui_->horizontalSlider_A->setEnabled(false);
+		ui_->horizontalSlider_B->setEnabled(false);
+		ui_->label_idA->setText("NaN");
+		ui_->label_idB->setText("NaN");
+	}
+}
+
+void MainWindow::cleanDatabase()
+{
+	if(!memory_ || !ids_.size())
+	{
+		QMessageBox::warning(this, tr("Cannot clean the database"), tr("A database (not empty) must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	bool ok;
+	int depth = QInputDialog::getInt(this, tr("Depth around the weighted locations?"), tr("Depth"), 10, 1, 1000, 1, &ok);
+	if(ok)
+	{
+		UINFO("Cleaning...");
+		//Remove all signatures with null weight and between two intersections
+		memory_->cleanLTM(depth);
+
+		//dbDriver_->executeNoResult(std::string("DELETE FROM Signature WHERE loopClosureId!=0;"));
+		//dbDriver_->executeNoResult(std::string("DELETE FROM Neighbor WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = Neighbor.sid);"));
+		//dbDriver_->executeNoResult(std::string("DELETE FROM Neighbor WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = Neighbor.nid);"));
+
+		// Clean links
+		//dbDriver_->executeNoResult(std::string("DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT * FROM Signature WHERE Signature.id = signatureId);"));
+
+		// Clean words
+		//dbDriver_->deleteUnreferencedWords();
+
+		updateIds();
+		UINFO("Finished cleaning!");
+	}
+}
+
+void MainWindow::cleanLocalGraph()
+{
+	if(!ids_.size() || !memory_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
+
+	if(ok)
+	{
+		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		if(ok)
+		{
+			UTimer timer;
+			UINFO("Cleaning local graph for location %d", id);
+
+			memory_->cleanLocalGraph(id, margin);
+
+			updateIds();
+			UINFO("time=%fs", timer.ticks());
+		}
+	}
+}
+
+void MainWindow::generateGraph()
+{
+	if(!memory_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
+	if(!path.isEmpty())
+	{
+		memory_->generateGraph(path.toStdString());
+	}
+}
+
+void MainWindow::generateLocalGraph()
+{
+	if(!ids_.size() || !memory_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
+
+	if(ok)
+	{
+		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		if(ok)
+		{
+			QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
+			if(!path.isEmpty())
+			{
+				double dbAccessTime = 0.0;
+				std::map<int, int> ids = memory_->getNeighborsId(dbAccessTime, id, margin, -1, false, false, false);
+
+				if(ids.size() > 0)
+				{
+					ids.insert(std::pair<int,int>(id, 0));
+					std::set<int> idsSet;
+					for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+					{
+						idsSet.insert(idsSet.end(), iter->first);
+						UINFO("Node %d", iter->first);
+					}
+					UINFO("idsSet=%d", idsSet.size());
+					memory_->generateGraph(path.toStdString(), idsSet);
+				}
+				else
+				{
+					QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
+				}
+			}
+		}
+	}
+}
+
+void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene)
+{
+	if(!scene)
+	{
+		return;
+	}
+	rtabmap::KeypointItem * item = 0;
+	int alpha = 70;
+	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
+	{
+		const cv::KeyPoint & r = (*i).second;
+		int id = (*i).first;
+		QString info = QString( "WordRef = %1\n"
+								"Laplacian = %2\n"
+								"Dir = %3\n"
+								"Hessian = %4\n"
+								"X = %5\n"
+								"Y = %6\n"
+								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
+		float radius = r.size*1.2/9.*2;
+
+		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+
+		scene->addItem(item);
+		item->setZValue(1);
+	}
+}
+
+void MainWindow::sliderAValueChanged(int value)
+{
+	this->update(value,
+			ui_->label_indexA,
+			ui_->label_actionsA,
+			ui_->label_parentsA,
+			ui_->label_childrenA,
+			ui_->graphicsView_A,
+			ui_->label_idA);
+}
+
+void MainWindow::sliderBValueChanged(int value)
+{
+	this->update(value,
+			ui_->label_indexB,
+			ui_->label_actionsB,
+			ui_->label_parentsB,
+			ui_->label_childrenB,
+			ui_->graphicsView_B,
+			ui_->label_idB);
+}
+
+void MainWindow::update(int value,
+						QLabel * labelIndex,
+						QLabel * labelActions,
+						QLabel * labelParents,
+						QLabel * labelChildren,
+						QGraphicsView * view,
+						QLabel * labelId)
+{
+	UTimer timer;
+	labelIndex->setText(QString::number(value));
+	labelActions->clear();
+	labelParents->clear();
+	labelChildren->clear();
+	if(value >= 0 && value < ids_.size())
+	{
+		view->scene()->clear();
+		int id = ids_.at(value);
+		labelId->setText(QString::number(id));
+		if(id>0)
+		{
+			//image
+			QImage img;
+			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
+			if(iter == imagesMap_.end())
+			{
+				if(memory_)
+				{
+					std::list<rtabmap::Sensor> sensors = memory_->getRawData(id);
+					if(sensors.size())
+					{
+						std::list<rtabmap::Sensor>::const_iterator jter = sensors.begin();
+						for(; jter!=sensors.end(); ++jter)
+						{
+							if(jter->type() == rtabmap::Sensor::kTypeImage)
+							{
+								break; //Stop to first
+							}
+						}
+						if(jter != sensors.end())
+						{
+							IplImage iplImg = jter->data();
+							img = ipl2QImage(&iplImg);
+							if(!img.isNull())
+							{
+								QByteArray ba;
+								QBuffer buffer(&ba);
+								buffer.open(QIODevice::WriteOnly);
+								img.save(&buffer, "BMP"); // writes image into ba in BMP format
+								imagesMap_.insert(id, ba);
+							}
+						}
+					}
+				}
+			}
+			else
+			{
+				img.loadFromData(iter.value(), "BMP");
+			}
+
+			if(memory_ && dynamic_cast<rtabmap::KeypointMemory*>(memory_))
+			{
+				std::multimap<int, cv::KeyPoint> words = dynamic_cast<rtabmap::KeypointMemory*>(memory_)->getWords(id);
+				if(words.size())
+				{
+					drawKeypoints(words, view->scene());
+				}
+			}
+
+			if(!img.isNull())
+			{
+				view->scene()->addPixmap(QPixmap::fromImage(img));
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image is empty");
+			}
+
+			// actions
+			if(id-1 > 0)
+			{
+				std::list<rtabmap::NeighborLink> links = memory_->getNeighborLinks(id-1, true, true);
+				for(std::list<rtabmap::NeighborLink>::iterator iter = links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->toId()>id-1 && iter->actuators().size())
+					{
+						QString str;
+						const std::list<rtabmap::Actuator> & actuators = iter->actuators();
+						unsigned int j=0;
+						for(std::list<rtabmap::Actuator>::const_iterator jter=actuators.begin(); jter!=actuators.end(); ++jter)
+						{
+							if(jter->data().elemSize() == 1)
+							{
+								for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
+								{
+									str.append(QString("%1 ").arg(*(char*)(jter->data().data + i)));
+								}
+							}
+							else if(jter->data().elemSize() == 2)
+							{
+								for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
+								{
+									str.append(QString("%1 ").arg(*(short*)(jter->data().data + i)));
+								}
+							}
+							else if(jter->data().elemSize() == 4)
+							{
+								for(unsigned int i=0; i<jter->data().total() * jter->data().elemSize(); i+=jter->data().elemSize())
+								{
+									if(jter->data().type() & CV_32F)
+									{
+										str.append(QString("%1 ").arg(*(float*)(jter->data().data + i)));
+									}
+									else
+									{
+										str.append(QString("%1 ").arg(*(int*)(jter->data().data + i)));
+									}
+								}
+							}
+							else
+							{
+								UERROR("not handled element size %d", jter->data().elemSize());
+								break;
+							}
+							if(j+1 < actuators.size())
+							{
+								str.append(QString("\n"));
+							}
+							++j;
+						}
+						if(str.size())
+						{
+							labelActions->setText(str);
+						}
+						break;
+					}
+				}
+			}
+
+			// loops
+			std::set<int> parents;
+			std::set<int> children;
+			memory_->getLoopClosureIds(id, parents, children, true);
+			if(parents.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				labelParents->setText(str);
+			}
+			if(children.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				labelChildren->setText(str);
+			}
+		}
+
+		labelId->setText(QString::number(id));
+		view->fitInView(view->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+	}
+	else
+	{
+		ULOGGER_ERROR("Slider index out of range ?");
+	}
+	UINFO("Time = %fs", timer.ticks());
+}
+
+void MainWindow::sliderAMoved(int value)
+{
+	ui_->label_indexA->setText(QString::number(value));
+	if(value>=0 && value < ids_.size())
+	{
+		ui_->label_idA->setText(QString::number(ids_.at(value)));
+	}
+	else
+	{
+		ULOGGER_ERROR("Slider index out of range ?");
+	}
+}
+
+void MainWindow::sliderBMoved(int value)
+{
+	ui_->label_indexB->setText(QString::number(value));
+	if(value>=0 && value < ids_.size())
+	{
+		ui_->label_idB->setText(QString::number(ids_.at(value)));
+	}
+	else
+	{
+		ULOGGER_ERROR("Slider index out of range ?");
+	}
+}
+
+QImage MainWindow::ipl2QImage(const IplImage *newImage)
+{
+	QImage qtemp;
+	if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
+	{
+		int x;
+		int y;
+		char* data = newImage->imageData;
+
+		qtemp= QImage(newImage->width, newImage->height,QImage::Format_RGB32 );
+		for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
+		{
+			for( x = 0; x < newImage->width; x++)
+			{
+				uint *p = (uint*)qtemp.scanLine (y) + x;
+				*p = qRgb(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels]);
+			}
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Wrong IplImage format");
+	}
+ return qtemp;
+}

tools/DatabaseViewer/MainWindow.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef MAINWINDOW_H_
+#define MAINWINDOW_H_
+
+#include <QtGui/QMainWindow>
+#include <QtCore/QByteArray>
+#include <QtCore/QMap>
+#include <QtCore/QSet>
+#include <QtGui/QImage>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <set>
+
+class Ui_MainWindow;
+class QGraphicsScene;
+class QGraphicsView;
+class QLabel;
+
+namespace rtabmap
+{
+class Memory;
+}
+
+class MainWindow : public QMainWindow
+{
+	Q_OBJECT
+
+public:
+	MainWindow(QWidget * parent = 0);
+	virtual ~MainWindow();
+
+private slots:
+	void openDatabase();
+	void cleanDatabase();
+	void cleanLocalGraph();
+	void generateGraph();
+	void generateLocalGraph();
+	void sliderAValueChanged(int);
+	void sliderBValueChanged(int);
+	void sliderAMoved(int);
+	void sliderBMoved(int);
+
+private:
+	void updateIds();
+	QImage ipl2QImage(const IplImage *newImage);
+	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
+	void update(int value,
+				QLabel * labelIndex,
+				QLabel * labelActions,
+				QLabel * labelParents,
+				QLabel * labelChildren,
+				QGraphicsView * view,
+				QLabel * labelId);
+
+private:
+	Ui_MainWindow * ui_;
+	QMap<int, QByteArray> imagesMap_;
+	QList<int> ids_;
+	rtabmap::Memory * memory_;
+	QString pathDatabase_;
+};
+
+#endif /* MAINWINDOW_H_ */

tools/DatabaseViewer/main.cpp

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <QtGui/QApplication>
+#include "MainWindow.h"
+#include "utilite/ULogger.h"
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	QApplication * app = new QApplication(argc, argv);
+	MainWindow * mainWindow = new MainWindow();
+	mainWindow->showNormal();
+
+	// 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;
+
+	return 0;
+}

tools/DatabaseViewer/ui/MainWindow.ui

@@ -0,0 +1,324 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>720</width>
+    <height>471</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Database viewer</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <layout class="QHBoxLayout" name="horizontalLayout_3">
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_6" stretch="1,0,0">
+      <item>
+       <widget class="QGraphicsView" name="graphicsView_A"/>
+      </item>
+      <item>
+       <widget class="QScrollArea" name="scrollArea_2">
+        <property name="horizontalScrollBarPolicy">
+         <enum>Qt::ScrollBarAsNeeded</enum>
+        </property>
+        <widget class="QWidget" name="scrollAreaWidgetContents_2">
+         <property name="geometry">
+          <rect>
+           <x>0</x>
+           <y>0</y>
+           <width>344</width>
+           <height>81</height>
+          </rect>
+         </property>
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_actionsA_2">
+            <property name="text">
+             <string>Actions</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_actionsA">
+            <property name="text">
+             <string>Actions</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_parentsA_2">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_parentsA">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_childrenA_2">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_childrenA">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </widget>
+      </item>
+      <item>
+       <layout class="QHBoxLayout" name="horizontalLayout">
+        <item>
+         <layout class="QVBoxLayout" name="verticalLayout_4">
+          <item>
+           <widget class="QLabel" name="label_2">
+            <property name="text">
+             <string>Index :</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_3">
+            <property name="text">
+             <string>Id :</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QVBoxLayout" name="verticalLayout_3">
+          <item>
+           <widget class="QLabel" name="label_indexA">
+            <property name="text">
+             <string>indexA</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_idA">
+            <property name="text">
+             <string>idA</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <widget class="QSlider" name="horizontalSlider_A">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+          <property name="tickPosition">
+           <enum>QSlider::TicksAbove</enum>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_5" stretch="1,0,0">
+      <item>
+       <widget class="QGraphicsView" name="graphicsView_B"/>
+      </item>
+      <item>
+       <widget class="QScrollArea" name="scrollArea">
+        <property name="horizontalScrollBarPolicy">
+         <enum>Qt::ScrollBarAsNeeded</enum>
+        </property>
+        <widget class="QWidget" name="scrollAreaWidgetContents">
+         <property name="geometry">
+          <rect>
+           <x>0</x>
+           <y>0</y>
+           <width>344</width>
+           <height>81</height>
+          </rect>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_actionsA_4">
+            <property name="text">
+             <string>Actions</string>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_actionsB">
+            <property name="text">
+             <string>Actions</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_parentsA_4">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLabel" name="label_parentsB">
+            <property name="text">
+             <string>Parents</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_childrenA_3">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QLabel" name="label_childrenB">
+            <property name="text">
+             <string>Children</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </widget>
+      </item>
+      <item>
+       <layout class="QHBoxLayout" name="horizontalLayout_2">
+        <item>
+         <layout class="QVBoxLayout" name="verticalLayout">
+          <item>
+           <widget class="QLabel" name="label_5">
+            <property name="text">
+             <string>Index :</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_4">
+            <property name="text">
+             <string>Id :</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <layout class="QVBoxLayout" name="verticalLayout_2">
+          <item>
+           <widget class="QLabel" name="label_indexB">
+            <property name="text">
+             <string>indexB</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QLabel" name="label_idB">
+            <property name="text">
+             <string>idB</string>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </item>
+        <item>
+         <widget class="QSlider" name="horizontalSlider_B">
+          <property name="orientation">
+           <enum>Qt::Horizontal</enum>
+          </property>
+          <property name="tickPosition">
+           <enum>QSlider::TicksAbove</enum>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+     </layout>
+    </item>
+   </layout>
+  </widget>
+  <widget class="QMenuBar" name="menubar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>720</width>
+     <height>25</height>
+    </rect>
+   </property>
+   <widget class="QMenu" name="menuFile">
+    <property name="title">
+     <string>File</string>
+    </property>
+    <addaction name="actionOpen_database"/>
+    <addaction name="separator"/>
+    <addaction name="actionQuit"/>
+   </widget>
+   <widget class="QMenu" name="menuEdit">
+    <property name="title">
+     <string>Edit</string>
+    </property>
+    <addaction name="actionGenerate_graph_dot"/>
+    <addaction name="actionGenerate_local_graph_dot"/>
+    <addaction name="actionClean_database"/>
+    <addaction name="actionClean_local_graph"/>
+   </widget>
+   <addaction name="menuFile"/>
+   <addaction name="menuEdit"/>
+  </widget>
+  <widget class="QStatusBar" name="statusbar"/>
+  <action name="actionOpen_database">
+   <property name="text">
+    <string>Open database</string>
+   </property>
+  </action>
+  <action name="actionQuit">
+   <property name="text">
+    <string>Quit</string>
+   </property>
+  </action>
+  <action name="actionGenerate_graph_dot">
+   <property name="text">
+    <string>Generate graph (.dot) ...</string>
+   </property>
+  </action>
+  <action name="actionGenerate_graph_only_weighted_locations">
+   <property name="text">
+    <string>Generate graph (only weighted locations) ...</string>
+   </property>
+  </action>
+  <action name="actionClean_database">
+   <property name="text">
+    <string>Clean database</string>
+   </property>
+  </action>
+  <action name="actionGenerate_local_graph_dot">
+   <property name="text">
+    <string>Generate local graph (.dot) ...</string>
+   </property>
+  </action>
+  <action name="actionClean_local_graph">
+   <property name="text">
+    <string>Clean local graph ...</string>
+   </property>
+  </action>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

tools/EpipolarGeometry/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+INCLUDE(${QT_USE_FILE})
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${QT_LIBRARIES} 
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(epipolar_geometry WIN32 ${SRC_FILES})
+TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_corelib rtabmap_guilib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( epipolar_geometry 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-epipolar_geometry)
+
+INSTALL(TARGETS epipolar_geometry
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/EpipolarGeometry/main.cpp

@@ -0,0 +1,352 @@
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <iostream>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <utilite/UConversion.h>
+#include <utilite/UDirectory.h>
+#include <utilite/UStl.h>
+#include <utilite/UMath.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include "rtabmap/core/VWDictionary.h"
+#include "rtabmap/core/KeypointDescriptor.h"
+#include "rtabmap/core/KeypointDetector.h"
+#include "rtabmap/core/EpipolarGeometry.h"
+
+#include "rtabmap/gui/ImageView.h"
+#include "rtabmap/gui/qtipl.h"
+#include "rtabmap/gui/KeypointItem.h"
+#include <QtGui/QApplication>
+#include <QtGui/QGraphicsLineItem>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QHBoxLayout>
+#include <QtCore/QTime>
+
+
+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_);
+		IplImage img1 = image1;
+		IplImage img2 = image2;
+		view1_->setSceneRect(0,0,(float)image1.cols, (float)image1.rows);
+		view1_->setLinesShown(true);
+		view1_->setFeaturesShown(false);
+		view1_->scene()->addPixmap(QPixmap::fromImage(Ipl2QImage(&img1,128)))->setVisible(view1_->isImageShown());
+		view1_->scene()->addPixmap(QPixmap::fromImage(Ipl2QImage(&img2,128)))->setVisible(view1_->isImageShown());
+		drawKeypoints(words1, words2, status);
+	}
+protected:
+	virtual void showEvent(QShowEvent* event)
+	{
+		resizeEvent(0);
+	}
+	virtual void resizeEvent(QResizeEvent* event)
+	{
+		view1_->fitInView(view1_->sceneRect(), Qt::KeepAspectRatio);
+		view1_->resetZoom();
+	}
+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();
+		KeypointItem * item = 0;
+		int alpha = 10*255/100;
+		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 )
+		{
+			const cv::KeyPoint & r = (*i).second;
+			int id = (*i).first;
+
+			QString info = QString( "WordRef = %1\n"
+									"Laplacian = %2\n"
+									"Dir = %3\n"
+									"Hessian = %4\n"
+									"X = %5\n"
+									"Y = %6\n"
+									"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
+			float radius = r.size*1.2/9.*2;
+			if(uContains(loopWords, id))
+			{
+				// PINK = FOUND IN LOOP SIGNATURE
+				item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 0, 255, alpha));
+				//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>(r.pt, uValues(loopWords, id).begin()->pt));
+					inliers.push_back(status[j++]);
+				}
+			}
+			else if(refWords.count(id) > 1)
+			{
+				// YELLOW = NEW and multiple times
+				item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+			}
+			else
+			{
+				// GREEN = NEW
+				item = new KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(0, 255, 0, alpha));
+			}
+			item->setVisible(view1_->isFeaturesShown());
+			view1_->scene()->addItem(item);
+			item->setZValue(1);
+		}
+		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)
+		{
+			QGraphicsLineItem * item = view1_->scene()->addLine(
+					iter->first.x,
+					iter->first.y,
+					iter->second.x,
+					iter->second.y,
+					*jter?QPen(Qt::cyan):QPen(Qt::red));
+			item->setVisible(view1_->isLinesShown());
+			item->setZValue(1);
+			++jter;
+		}
+	}
+
+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]);
+		image2 = cv::imread(argv[2]);
+	}
+	else
+	{
+		showUsage();
+	}
+
+	QTime timer;
+	timer.start();
+
+	// Extract words
+	timer.start();
+   VWDictionary dictionary;
+   ParametersMap param;
+   param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
+   param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
+   SURFDetector keypointDetector(param);
+   SURFDescriptor descriptorExtractor(param);
+   std::vector<cv::KeyPoint> kpts1 = keypointDetector.generateKeypoints(image1);
+   std::vector<cv::KeyPoint> kpts2 = keypointDetector.generateKeypoints(image2);
+   cv::Mat descriptors1 = descriptorExtractor.generateDescriptors(image1, kpts1);
+   cv::Mat descriptors2 = descriptorExtractor.generateDescriptors(image2, kpts2);
+   UINFO("detect/extract features = %d ms", timer.elapsed());
+
+   timer.start();
+   std::list<int> wordIds1 = dictionary.addNewWords(descriptors1, 1);
+   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;
+   findPairsUnique(words1, words2, pairs);
+   UINFO("find pairs = %d ms", timer.elapsed());
+
+   // Find fundamental matrix
+   timer.start();
+   std::vector<uchar> status;
+   cv::Mat fundamentalMatrix = 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 = findPFromF(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;
+		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;
+}

tools/ImagesDbExtractor/CMakeLists.txt

@@ -0,0 +1,30 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(imagesDbExtractor ${SRC_FILES})
+TARGET_LINK_LIBRARIES(imagesDbExtractor rtabmap_corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( imagesDbExtractor 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesDbExtractor)
+
+INSTALL(TARGETS imagesDbExtractor
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/ImagesDbExtractor/main.cpp

@@ -0,0 +1,64 @@
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <iostream>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <utilite/UConversion.h>
+#include <utilite/UDirectory.h>
+#include "rtabmap/core/SMMemory.h"
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/LTM.db";
+	if(argc > 1)
+	{
+		path = argv[1];
+	}
+
+
+    // Open database
+    std::string driverType = "sqlite3";
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
+	rtabmap::SMMemory * memory = new rtabmap::SMMemory(parameters);
+	if(!memory)
+	{
+		UWARN("Can't create database driver \"%s\"", driverType.c_str());
+	}
+	else if(!memory->init(driverType, path))
+	{
+		UWARN("Can't open database \"%s\"", path.c_str());
+	}
+
+	if(memory)
+	{
+		UINFO("Using database %s", path.c_str());
+		std::string saveDirectory = "imagesExtracted/";
+		if(!UDirectory::exists(saveDirectory))
+		{
+			UDirectory::makeDir(saveDirectory);
+		}
+		std::set<int> ids = memory->getAllSignatureIds();
+		for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			std::list<rtabmap::Sensor> sensors = memory->getRawData(*iter);
+			for(std::list<rtabmap::Sensor>::const_iterator jter=sensors.begin(); jter!=sensors.end(); ++jter)
+			{
+				int k=0;
+				if(jter->type() == rtabmap::Sensor::kTypeImage)
+				{
+					std::string fileName = uFormat("%d-%d.png", *iter, k++);
+					cv::imwrite(saveDirectory+fileName, jter->data());
+					UINFO("Saved %s", (saveDirectory+fileName).c_str());
+				}
+			}
+		}
+	}
+
+    return 0;
+}

tools/ImagesJoiner/CMakeLists.txt

@@ -0,0 +1,32 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBS} 
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+# Add binary called "imagesJoiner" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(imagesJoiner ${SRC_FILES})
+TARGET_LINK_LIBRARIES(imagesJoiner ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( imagesJoiner 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesJoiner)
+
+INSTALL(TARGETS imagesJoiner
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/ImagesJoiner/main.cpp

@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UDirectory.h"
+#include "utilite/UConversion.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"imagesJoiner.exe \"# (see below)\" \"type\" [option]\n"
+			"  # :      Name pattern is how the file names are formatted (in number).\n"
+			"           Examples: 4 for pictures with 0001.jpg, 0002.jpg, ..., 0010.jpg, 0100.jpg, 1000.jpg, ...\n"
+			"                     1 for pictures with 1.jpg, 2.jpg, ..., 10.jpg, 100.jpg, 1000.jpg, ...\n"
+			"  type :   is the extension (jpg, bmp, png...)\n"
+			"  Options:\n"
+			"    -inv       option for copying odd images on the right\n"
+			"    -d #       destination filename size\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	if(argc < 3)
+	{
+		showUsage();
+	}
+
+	bool inv = false;
+	unsigned int sizeFileName = std::atoi(argv[1]);
+	int sizeTargetFileName = sizeFileName;
+
+	std::string type = argv[2];
+	for(int i=3; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-inv") == 0)
+		{
+			inv = true;
+			printf(" Inversing option activated...\n");
+		}
+		if(strcmp(argv[i], "-d") == 0 && i+1<argc)
+		{
+			sizeTargetFileName = std::atoi(argv[i+1]);
+			if(sizeTargetFileName < 0)
+			{
+				showUsage();
+			}
+			printf(" Target size file name option activated=%d\n",sizeTargetFileName);
+			++i;
+		}
+	}
+
+	printf(" Format = %s\n", argv[1]);
+	printf(" Type = %s\n", argv[2]);
+
+	std::string targetDirectory = "imagesJoined/";
+	UDirectory::makeDir((UDirectory::currentDir(true) + "imagesJoined").c_str());
+
+	int counterJoined = 1;
+	int counterImages = 1;
+	bool imagesExist = true;
+
+	std::string fileNameA;
+	std::string fileNameB;
+
+	while(imagesExist)
+	{
+		std::string fileNameTarget = uNumber2Str(counterJoined);
+		if(inv)
+		{
+			fileNameA =  uNumber2Str(counterImages+1);
+			fileNameB =  uNumber2Str(counterImages);
+		}
+		else
+		{
+			fileNameA =  uNumber2Str(counterImages);
+			fileNameB =  uNumber2Str(counterImages+1);
+		}
+
+		while(fileNameA.size() < sizeFileName)
+		{
+			fileNameA.insert(0, "0");
+		}
+
+		while(fileNameB.size() < sizeFileName)
+		{
+			fileNameB.insert(0, "0");
+		}
+
+		while(fileNameTarget.size() < (unsigned int)sizeTargetFileName)
+		{
+			fileNameTarget.insert(0, "0");
+		}
+
+		(fileNameTarget.insert(0, targetDirectory) += ".") += type;
+		(fileNameA += ".") += type;
+		(fileNameB += ".") += type;
+
+		IplImage * imageA = cvLoadImage(fileNameA.c_str(), CV_LOAD_IMAGE_COLOR);
+		IplImage * imageB = cvLoadImage(fileNameB.c_str(), CV_LOAD_IMAGE_COLOR);
+
+		if(imageA && imageB)
+		{
+			CvSize sizeA = cvGetSize(imageA);
+			CvSize sizeB = cvGetSize(imageB);
+			CvSize targetSize = {0};
+			targetSize.width = sizeA.width + sizeB.width;
+			targetSize.height = sizeA.height > sizeB.height ? sizeA.height : sizeB.height;
+			IplImage* targetImage = cvCreateImage(targetSize, imageA->depth, imageA->nChannels);
+			if(targetImage)
+			{
+				cvSetImageROI( targetImage, cvRect( 0, 0, sizeA.width, sizeA.height ) );
+				cvCopy( imageA, targetImage );
+				cvSetImageROI( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) );
+				cvCopy( imageB, targetImage );
+				cvResetImageROI( targetImage );
+
+				if(!cvSaveImage(fileNameTarget.c_str(), targetImage))
+				{
+					printf("Error : saving to \"%s\" goes wrong...\n", fileNameTarget.c_str());
+				}
+				else
+				{
+					printf("Saved \"%s\" \n", fileNameTarget.c_str());
+				}
+
+				cvReleaseImage(&targetImage);
+			}
+			else
+			{
+				printf("Error : can't allocated the target image with size (%d,%d)\n", targetSize.width, targetSize.height);
+				imagesExist = false;
+			}
+		}
+		else
+		{
+			imagesExist = false;
+		}
+
+		if(imageA)
+		{
+			cvReleaseImage(&imageA);
+		}
+		if(imageB)
+		{
+			cvReleaseImage(&imageB);
+		}
+
+		counterJoined++;
+		counterImages += 2;
+	}
+
+
+
+
+	return 0;
+}

tools/Polar/CMakeLists.txt

@@ -0,0 +1,27 @@
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${FFTW3F_INCLUDE_DIRS} 
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${FFTW3F_LIBRARIES} 
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(logPolar main.cpp)
+TARGET_LINK_LIBRARIES(logPolar rtabmap_corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( logPolar 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-logPolar)
+
+INSTALL(TARGETS logPolar
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/Polar/main.cpp

@@ -0,0 +1,144 @@
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
+#include <iostream>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <fftw3.h>
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/ColorTable.h"
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kDebug);
+
+    cv::Mat srcMat;
+    if( argc == 2)
+    {
+    	srcMat=cvLoadImage(argv[1],1);
+    }
+    else
+    {
+    	rtabmap::CameraVideo cam(0,0,false, 640, 480);
+    	if(cam.init())
+    	{
+    		srcMat = cam.takeImage();
+    	}
+    }
+    if(!srcMat.empty())
+    {
+    	UTimer timer;
+    	timer.start();
+    	IplImage srfRef = srcMat;
+    	IplImage * src = &srfRef;
+    	// Log-polar transform
+    	int radius = src->height < src->width ? src->height/2: src->width/2;
+    	CvSize polarSize = cvSize(64, 128);
+    	float M = polarSize.width/std::log(radius);
+    	UDEBUG("src size=(%d,%d) radius=%d, M=%f", src->width, src->height, radius, M);
+        IplImage* polar = cvCreateImage( polarSize, 8, 3 );
+        IplImage* src2 = cvCreateImage( cvGetSize(src), 8, 3 );
+        cvLogPolar( src, polar, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+        cvLogPolar( polar, src2, cvPoint2D32f(src->width/2,src->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+        UDEBUG("logpolar time=%fs", timer.ticks());
+
+        // HSV transform
+        IplImage* hsv = cvCreateImage( cvGetSize(polar), IPL_DEPTH_8U, 3 );
+        cvCvtColor( polar, hsv, CV_BGR2HSV );
+        UDEBUG("bgr->hsv time=%fs", timer.ticks());
+
+        // Fetch H channel
+        cv::Mat hsvMat(hsv);
+        cv::vector<cv::Mat> channels;
+        split(hsvMat, channels);
+        cv::Mat hMat;
+        channels[0].convertTo(hMat, CV_32F);
+        hMat /= 255.0f;
+        UDEBUG("fetch h channel time=%fs", timer.ticks());
+
+        // DFT transform
+        cv::Mat dftMat;
+        cv::dft(hMat, dftMat, cv::DFT_ROWS);
+        UDEBUG("dft opencv time=%fs", timer.ticks());
+
+        // FFT transform
+        float in[hMat.cols];
+        fftwf_complex * out;
+		fftwf_plan p;
+		cv::Mat fftMat(hMat.rows, hMat.cols/2+1, CV_32F);
+		out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * fftMat.cols);
+		p = fftwf_plan_dft_r2c_1d(hMat.cols, in, out, 0);
+		UDEBUG("fft create plan time=%fs", timer.ticks());
+		if(p==0)
+		{
+			UFATAL("cannot create a plan");
+		}
+
+		for(int i=0; i<hMat.rows; ++i)
+		{
+			cv::Mat hRow = hMat(cv::Range(i,i+1), cv::Range(0,hMat.cols));
+			memcpy(in, hRow.data, sizeof(float)*hRow.cols);
+
+			fftwf_execute(p); /* repeat as needed */
+			for(int j=0; j<fftMat.cols; ++j)
+			{
+				cv::Mat fftRow = fftMat(cv::Range(i,i+1), cv::Range(0,fftMat.cols));
+				float re = (float)out[j][0];
+				float im = (float)out[j][1];
+				fftRow.at<float>(0,j) = sqrt(re*re+im*im); // TODO keep only real of the complex instead of the module ?
+			}
+		}
+		UDEBUG("fft time=%fs", timer.ticks());
+
+		fftwf_destroy_plan(p);
+		fftwf_free(out);
+		UDEBUG("fft cleanup time=%fs", timer.ticks());
+
+		//std::cout << dftMat << std::endl;
+		//std::cout << fftMat << std::endl;
+
+
+        //UDEBUG("hMat row=%d cols=%d", hMat.rows, hMat.cols);
+        //UDEBUG("hMat row=%d cols=%d, channels=%d", dftMat.rows, dftMat.cols, dftMat.channels());
+
+
+		IplImage * ind = cvCloneImage(src);
+		unsigned char * imageData = (unsigned char *)ind->imageData;
+		rtabmap::ColorTable colorTable(65536);
+		UDEBUG("widthStep=%d", ind->widthStep);
+		for(int i=0; i<ind->height; ++i)
+		{
+			for(int j=0; j<ind->width; ++j)
+			{
+				unsigned char & b = imageData[i*ind->widthStep+j*3+0];
+				unsigned char & g = imageData[i*ind->widthStep+j*3+1];
+				unsigned char & r = imageData[i*ind->widthStep+j*3+2];
+				int index = (int)colorTable.getIndex(r, g, b);
+				colorTable.getRgb(index, r, g , b);
+			}
+		}
+
+        cvNamedWindow( "log-original", 1 );
+		cvShowImage( "log-original", src );
+		cvNamedWindow( "log-polar", 1 );
+		cvShowImage( "log-polar", polar );
+		cvNamedWindow( "inverse log-polar", 1 );
+		cvShowImage( "inverse log-polar", src2 );
+		cvNamedWindow( "hsv", 1 );
+		cvShowImage( "hsv", hsv );
+		cvNamedWindow( "ind", 1 );
+		cvShowImage( "ind", ind );
+
+		UDEBUG("show time=%fs", timer.ticks());
+
+        cvWaitKey();
+
+        cvReleaseImage(&polar);
+        cvReleaseImage(&src2);
+        cvReleaseImage(&hsv);
+        cvReleaseImage(&ind);
+    }
+    return 0;
+}

tools/WebcamCapture/CMakeLists.txt

@@ -0,0 +1,34 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARIES}
+	${OpenCV_LIBS} 
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+# Add binary called "imagesJoiner" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(webcamCapture ${SRC_FILES})
+TARGET_LINK_LIBRARIES(webcamCapture rtabmap_corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( webcamCapture 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-webcamCapture)
+
+INSTALL(TARGETS webcamCapture
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+

tools/WebcamCapture/main.cpp

@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UDirectory.h"
+#include "utilite/UConversion.h"
+#include "rtabmap/core/Camera.h"
+#include "utilite/UEventsManager.h"
+#include "utilite/UEventsHandler.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <iostream>
+
+void showUsage()
+{
+	printf("Usage:\n"
+			"webcamCapture [option]\n"
+			"   -device #:			Id of the webcam (default 0)\n"
+			"   -width #:			Image width (default 640)\n"
+			"   -height #:			Image height (default 480)\n"
+			"   -fps #.#:			Frame rate (Hz) (default 2.0)\n"
+			"   -hide :			    Image not shown while capturing (default true)\n"
+			"   -dir \"path\":		Path of the images saved (default \"./imagesCaptured\")\n"
+			"   -save :			    Save images captured (default true)\n"
+			"   -startId #:			Image id to start with (default 1)\n"
+			"   -ext \"ext\":		Image extension (default \"jpg\")\n"
+			"   -debug:				Debug trace\n");
+	exit(1);
+}
+
+class ImagesHandler : public UEventsHandler
+{
+public:
+	ImagesHandler(const std::string & targetDir, const std::string & ext, int startId,  bool show, bool save) :
+		targetDir_(targetDir),
+		ext_(ext),
+		startId_(startId),
+		show_(show),
+		save_(save),
+		id_(startId)
+	{
+		if(show_)
+		{
+			cv::namedWindow("Webcam", CV_WINDOW_AUTOSIZE);
+		}
+	}
+	virtual ~ImagesHandler()
+	{
+		if(show_)
+		{
+			cv::destroyWindow("Webcam");
+		}
+	}
+
+protected:
+	virtual void handleEvent(UEvent * e)
+	{
+		if(e->getClassName().compare("SMStateEvent") == 0)
+		{
+			const rtabmap::CameraEvent * event = (const rtabmap::CameraEvent*)e;
+			cv::Mat image = event->image();
+			if(!image.empty())
+			{
+				if(show_)
+				{
+					cv::imshow("Webcam", image);
+				}
+				if(save_)
+				{
+					std::string fileName = targetDir_ + "/";
+					fileName += uNumber2Str(id_++);
+					fileName += ".";
+					fileName += ext_;
+					cv::imwrite(fileName, image);
+					printf("Image %s saved!\n", fileName.c_str());
+				}
+			}
+			else
+			{
+				printf("Image is null?!?\n");
+			}
+		}
+
+	}
+private:
+	std::string targetDir_;
+	std::string ext_;
+	int startId_;
+	bool show_;
+	bool save_;
+	int id_;
+};
+
+int main(int argc, char * argv[])
+{
+	bool show = true;
+	int usbDevice = 0;
+	int imageWidth = 640;
+	int imageHeight = 480;
+	float imageRate = 2.0;
+	int startId = 1;
+	std::string extension = "jpg";
+	bool save = false;
+	std::string targetDirectory = UDirectory::currentDir(true) + "imagesCaptured";
+
+	for(int i=1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-h") == 0 ||
+			strcmp(argv[i], "-help") == 0 ||
+			strcmp(argv[i], "?") == 0)
+		{
+			showUsage();
+		}
+
+		if(strcmp(argv[i], "-device") == 0 && i+1<argc)
+		{
+			usbDevice = std::atoi(argv[i+1]);
+			if(usbDevice < 0 || usbDevice > 100)
+			{
+				showUsage();
+			}
+			++i;
+		}
+
+		if(strcmp(argv[i], "-width") == 0 && i+1<argc)
+		{
+			imageWidth = std::atoi(argv[i+1]);
+			if(imageWidth < 0)
+			{
+				showUsage();
+			}
+			++i;
+		}
+
+		if(strcmp(argv[i], "-height") == 0 && i+1<argc)
+		{
+			imageHeight = std::atoi(argv[i+1]);
+			if(imageHeight < 0)
+			{
+				showUsage();
+			}
+			++i;
+		}
+
+		if(strcmp(argv[i], "-hz") == 0 && i+1<argc)
+		{
+			imageRate = std::atof(argv[i+1]);
+			if(imageRate < 0)
+			{
+				showUsage();
+			}
+			++i;
+		}
+
+		if(strcmp(argv[i], "-hide") == 0 && i+1<argc)
+		{
+			show = false;
+		}
+
+		if(strcmp(argv[i], "-dir") == 0 && i+1<argc)
+		{
+			targetDirectory = argv[i+1];
+			++i;
+		}
+
+		if(strcmp(argv[i], "-save") == 0 && i+1<argc)
+		{
+			save = true;
+		}
+		
+		if(strcmp(argv[i], "-debug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+			ULogger::setType(ULogger::kTypeConsole);
+		}
+		
+		if(strcmp(argv[i], "-startId") == 0 && i+1<argc)
+		{
+			startId = std::atoi(argv[i+1]);
+			++i;
+		}
+		
+		if(strcmp(argv[i], "-ext") == 0 && i+1<argc)
+		{
+			extension = argv[i+1];
+			++i;
+		}
+	}
+
+	printf("Parameters:\n"
+		   "   device=%d\n"
+		   "   width=%d\n"
+		   "   height=%d\n"
+		   "   hz=%f\n"
+		   "   show=%s\n"
+		   "   dir=%s\n"
+		   "   extension=%s\n"
+		   "   startId=%d\n"
+		   "   save=%s\n", 
+		   usbDevice, 
+		   imageWidth, 
+		   imageHeight, 
+		   imageRate, 
+		   uBool2Str(show).c_str(),
+		   targetDirectory.c_str(),
+		   extension.c_str(),
+		   startId,
+		   uBool2Str(save).c_str());
+
+	UDirectory::makeDir(targetDirectory);
+
+	rtabmap::CameraVideo cam(usbDevice, imageRate, false, imageWidth, imageHeight);
+	if(!cam.init())
+	{
+		printf("Can't initialize the camera...\n");
+		return 1;
+	}
+
+	ImagesHandler imgHandler(targetDirectory, extension, startId, show, save);
+	UEventsManager::addHandler(&imgHandler);
+
+	cam.start();
+
+	if(show)
+	{
+		cv::waitKey(0);
+	}
+	else
+	{
+		std::cin.ignore();
+	}
+
+	return 0;
+}