moved rtabmap-ros-pkg project in this project

git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@52 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/CMakeLists.txt

@@ -0,0 +1,16 @@
+ADD_SUBDIRECTORY( src )
+ADD_SUBDIRECTORY( ConsoleApp )
+ADD_SUBDIRECTORY( ImagesJoiner )
+ADD_SUBDIRECTORY( WebcamCapture )
+
+IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+	ADD_SUBDIRECTORY( DatabaseViewer )
+ELSE()
+   MESSAGE(STATUS "[WARNING] Qt4 not found, the databaseViewer program will not be built...")
+ENDIF()
+
+IF(CPPUNIT_FOUND)
+	ADD_SUBDIRECTORY( tests )
+ELSE(CPPUNIT_FOUND)
+   MESSAGE(STATUS "CppUnit is not found, tests for the ${PROJECT_NAME} project won't be compiled...")
+ENDIF(CPPUNIT_FOUND)

corelib/ConsoleApp/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${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 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)
+

corelib/ConsoleApp/main.cpp

@@ -0,0 +1,560 @@
+/*
+ * 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 "rtabmap/core/SMState.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.txt"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-console [options] \"path\"\n"
+			"  path                            For images, use the directory path. For videos, use full\n "
+			"                                  path name\n"
+			"Options:\n"
+			"  -t #.##                         Time threshold (seconds)\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 of dim # (>0, must match the size\n"
+			"                                   of the data set)\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"
+			"  -\"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;
+	int createGT = 0;
+	int imageWidth = 0;
+	int imageHeight = 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], "-createGT") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				createGT = std::atoi(argv[i]);
+				if(createGT < 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			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, ',', ' ');
+				}
+				pm.insert(ParametersPair(key, 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, 1, false, 0.0f, false, imageWidth, imageHeight);
+	}
+	else
+	{
+		camera = new CameraVideo(path, 0.0f, false, imageWidth, imageHeight);
+	}
+
+	if(!camera || !camera->init())
+	{
+		printf("Camera init failed, using path \"%s\"\n", path.c_str());
+		exit(1);
+	}
+
+
+	CvMat * groundTruthMat = 0;
+	if(createGT)
+	{
+		printf("Creating the ground truth matrix...%dx%d\n", createGT, createGT);
+		groundTruthMat = cvCreateMat(createGT, createGT, CV_32FC1);
+	}
+
+
+	// Create tasks
+	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->init();
+	rtabmap->setMaxTimeAllowed(timeThreshold); // in sec
+
+	//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
+	IplImage * image = 0;
+	int loopClosureId;
+	int count = 0;
+	int countLoopDetected=0;
+
+	printf("\nParameters : \n");
+	printf(" Data set : %s\n", path.c_str());
+	printf(" Time threshold = %1.2f\n", timeThreshold);
+	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
+	printf(" Repeating dataset = %s\n", repeat?"true":"false");
+	printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
+	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());
+		}
+	}
+	printf("\nProcessing images...\n");
+
+	UTimer iterationTimer;
+	int imagesProcessed = 0;
+	std::list<std::vector<float> > teleopActions;
+	int maxTeleopActions = 0; // TEST Lip6Indoor with 190, 0->disabled
+	std::list<std::vector<float> > actions;
+	while(loopDataset <= repeat && g_forever)
+	{
+		image = camera->takeImage();
+		int i=0;
+		while(image && g_forever)
+		{
+			++imagesProcessed;
+			iterationTimer.start();
+			SMState * smState;
+			if(i<maxTeleopActions)
+			{
+				// ONLY TESTING HERE if maxTeleopActions>0
+				std::vector<float> v(5);
+				v[0] = 1;
+				v[1] = 16;
+				v[2] = 32;
+				v[3] = 64;
+				v[4] = 128;
+				teleopActions.push_back(v);
+				smState = new SMState(image, teleopActions);
+			}
+			else
+			{
+				smState = new SMState(image, actions);
+			}
+			rtabmap->process(smState);
+			loopClosureId = rtabmap->getLoopClosureId();
+			actions = rtabmap->getActions();
+			if(rtabmap->getLoopClosureId())
+			{
+				++countLoopDetected;
+			}
+			image = camera->takeImage();
+			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(groundTruthMat)
+			{
+				if(loopClosureId > 0 && loopClosureId-1 < groundTruthMat->cols)
+				{
+					cvmSet(groundTruthMat, i, loopClosureId-1, 1);
+				}
+			}
+
+			++i;
+
+			double iterationTime = iterationTimer.ticks();
+
+			ULogger::flush();
+
+			if(rate)
+			{
+				float delta = rate - iterationTime;
+				if(delta > 0)
+				{
+					uSleep(delta*1000);
+				}
+			}
+			if(rtabmap->getLoopClosureId())
+			{
+				printf(" iteration(%d) actions=%d loop(%d) time=%fs\n", count, (int)actions.size(), rtabmap->getLoopClosureId(), iterationTime);
+			}
+			else
+			{
+				printf(" iteration(%d) actions=%d time=%fs\n", count, (int)actions.size(), 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(groundTruthMat)
+	{
+		if(rtabmap->getTotalMemSize() != groundTruthMat->rows)
+		{
+			printf("WARNING : Ground truth matrix size and the image count don't match : Image captured=%d, GroundTruthSize = %d\n", imagesProcessed, groundTruthMat->rows);
+		}
+
+		// 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);
+		FILE* fout = 0;
+#ifdef _MSC_VER
+		fopen_s(&fout, (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), "w+");
+#else
+		fout = fopen((rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), "w+");
+#endif
+		if(fout)
+		{
+			for(int i=0; i<groundTruthMat->rows; i++)
+			{
+				for(int j=0; j<groundTruthMat->cols; j++)
+				{
+					fprintf(fout, "%d", cvmGet(groundTruthMat,i,j)>0?255:0);
+					if(j+1<groundTruthMat->cols)
+					{
+						fprintf(fout," ");
+					}
+				}
+				if(i+1<groundTruthMat->rows)
+				{
+					fprintf(fout,"\n");
+				}
+			}
+			fclose(fout);
+			fout = 0;
+		}
+		else
+		{
+			printf("ERROR : Can't generate the ground truth file \"%s\"...\n", (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str());
+		}
+		cvReleaseMat(&groundTruthMat);
+		groundTruthMat = 0;
+		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;
+}

corelib/DatabaseViewer/CMakeLists.txt

@@ -0,0 +1,57 @@
+### 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
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+	${CMAKE_CURRENT_SOURCE_DIR}/../src
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${UTILITE_INCLUDE_DIR}
+	${OpenCV_INCLUDE_DIRS}
+	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
+)
+
+INCLUDE(${QT_USE_FILE})
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+    ${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 corelib ${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)

corelib/DatabaseViewer/MainWindow.cpp

@@ -0,0 +1,417 @@
+/*
+ * 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 "KeypointMemory.h"
+#include "rtabmap/core/DBDriver.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()
+{
+	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"));
+		memory_ = new rtabmap::KeypointMemory(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_ = path;
+			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())
+			{
+				std::map<int, int> ids;
+				memory_->getNeighborsId(ids, id, margin-1, true);
+				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);
+				}
+				memory_->generateGraph(path.toStdString(), idsSet);
+			}
+		}
+	}
+}
+
+void MainWindow::sliderAValueChanged(int value)
+{
+	ui_->label_indexA->setText(QString::number(value));
+	if(value >= 0 && value < ids_.size())
+	{
+		ui_->graphicsView_A->scene()->clear();
+		int id = ids_.at(value);
+		ui_->label_idA->setText(QString::number(id));
+		if(id>0)
+		{
+			QImage img;
+			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
+			if(iter == imagesMap_.end())
+			{
+				if(memory_)
+				{
+					IplImage * image = memory_->getImage(id);
+					if(image)
+					{
+						img = ipl2QImage(image);
+						cvReleaseImage(&image);
+						if(!img.isNull())
+						{
+							QByteArray ba;
+							QBuffer buffer(&ba);
+							buffer.open(QIODevice::WriteOnly);
+							img.save(&buffer, "JPEG"); // writes image into ba in JPEG format
+							imagesMap_.insert(id, ba);
+						}
+					}
+				}
+			}
+			else
+			{
+				img.loadFromData(iter.value(), "JPEG");
+			}
+
+			if(!img.isNull())
+			{
+				ui_->graphicsView_A->scene()->addPixmap(QPixmap::fromImage(img));
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image is empty");
+			}
+		}
+
+		ui_->label_idA->setText(QString::number(id));
+		ui_->graphicsView_A->fitInView(ui_->graphicsView_A->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+	}
+	else
+	{
+		ULOGGER_ERROR("Slider index out of range ?");
+	}
+}
+
+void MainWindow::sliderBValueChanged(int value)
+{
+	ui_->label_indexB->setText(QString::number(value));
+	if(value >= 0 && value < ids_.size())
+	{
+		ui_->graphicsView_B->scene()->clear();
+		int id = ids_.at(value);
+		ui_->label_idB->setText(QString::number(id));
+		if(id>0)
+		{
+			QImage img;
+			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
+			if(iter == imagesMap_.end())
+			{
+				if(memory_)
+				{
+					IplImage * image = memory_->getImage(id);
+					if(image)
+					{
+						img = ipl2QImage(image);
+						cvReleaseImage(&image);
+
+						if(!img.isNull())
+						{
+							QByteArray ba;
+							QBuffer buffer(&ba);
+							buffer.open(QIODevice::WriteOnly);
+							img.save(&buffer, "JPEG"); // writes image into ba in JPEG format
+							imagesMap_.insert(id, ba);
+						}
+					}
+				}
+			}
+			else
+			{
+				img.loadFromData(iter.value(), "JPEG");
+			}
+
+			if(!img.isNull())
+			{
+				ui_->graphicsView_B->scene()->addPixmap(QPixmap::fromImage(img));
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image is empty");
+			}
+		}
+
+		ui_->label_idB->setText(QString::number(id));
+		ui_->graphicsView_B->fitInView(ui_->graphicsView_B->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
+	}
+	else
+	{
+		ULOGGER_ERROR("Slider index out of range ?");
+	}
+}
+
+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) //fct recuperer sur le net, converti un ldImage en QImage
+{
+	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;
+}

corelib/DatabaseViewer/MainWindow.h

@@ -0,0 +1,69 @@
+/*
+ * 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 <set>
+
+class Ui_MainWindow;
+
+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);
+
+private:
+	Ui_MainWindow * ui_;
+	QMap<int, QByteArray> imagesMap_;
+	QList<int> ids_;
+	rtabmap::Memory * memory_;
+	QString pathDatabase_;
+};
+
+#endif /* MAINWINDOW_H_ */

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

corelib/DatabaseViewer/ui/MainWindow.ui

@@ -0,0 +1,202 @@
+<?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>550</width>
+    <height>318</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Images database viewer</string>
+  </property>
+  <widget class="QWidget" name="centralwidget">
+   <layout class="QHBoxLayout" name="horizontalLayout_3">
+    <item>
+     <layout class="QVBoxLayout" name="verticalLayout_6">
+      <item>
+       <widget class="QGraphicsView" name="graphicsView_A"/>
+      </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">
+      <item>
+       <widget class="QGraphicsView" name="graphicsView_B"/>
+      </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>550</width>
+     <height>22</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>

corelib/ImagesJoiner/CMakeLists.txt

@@ -0,0 +1,32 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${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)
+

corelib/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]);
+	unsigned 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() < 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;
+}

corelib/WebcamCapture/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+SET(SRC_FILES
+    main.cpp
+)
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${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 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)
+

corelib/WebcamCapture/main.cpp

@@ -0,0 +1,260 @@
+/*
+ * 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 "rtabmap/core/SMState.h"
+#include "utilite/UEventsManager.h"
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/CameraEvent.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"
+			"   -show bool:			Image shown while capturing (default true)\n"
+			"   -dir \"path\":		Path of the images saved (default \"./imagesCaptured\")\n"
+			"   -save bool:			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_)
+		{
+			cvNamedWindow("Webcam", CV_WINDOW_AUTOSIZE);
+		}
+	}
+	virtual ~ImagesHandler()
+	{
+		if(show_)
+		{
+			cvDestroyWindow("Webcam");
+		}
+	}
+
+protected:
+	virtual void handleEvent(UEvent * e)
+	{
+		if(e->getClassName().compare("SMStateEvent") == 0)
+		{
+			const rtabmap::SMStateEvent * event = (const rtabmap::SMStateEvent*)e;
+			const rtabmap::SMState * sm = event->getData();
+			const IplImage * image = 0;
+			if(sm)
+			{
+				image = sm->getImage();
+			}
+			if(image)
+			{
+				if(show_)
+				{
+					cvShowImage("Webcam", image);
+				}
+				if(save_)
+				{
+					std::string fileName = targetDir_ + "/";
+					fileName += uNumber2str(id_++);
+					fileName += ".";
+					fileName += ext_;
+					cvSaveImage(fileName.c_str(), 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 = true;
+	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], "-show") == 0 && i+1<argc)
+		{
+			show = uStr2Bool(argv[i+1]);
+			++i;
+		}
+
+		if(strcmp(argv[i], "-dir") == 0 && i+1<argc)
+		{
+			targetDirectory = argv[i+1];
+			++i;
+		}
+
+		if(strcmp(argv[i], "-save") == 0 && i+1<argc)
+		{
+			save = uStr2Bool(argv[i+1]);
+			++i;
+		}
+		
+		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, imageWidth, imageHeight, false, imageRate);
+	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)
+	{
+		cvWaitKey(0);
+	}
+	else
+	{
+		std::cin.ignore();
+	}
+
+	return 0;
+}

corelib/include/rtabmap/core/Camera.h

@@ -0,0 +1,237 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include "utilite/UThreadNode.h"
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+#include <set>
+#include <stack>
+
+class UDirectory;
+
+namespace rtabmap
+{
+
+class KeypointDetector;
+class KeypointDescriptor;
+class SMState;
+
+/**
+ * Only encapsulate the image in a newly created SMState.
+ */
+class RTABMAP_EXP CamPostTreatment
+{
+public:
+	CamPostTreatment(const ParametersMap & parameters = ParametersMap()) {
+		this->parseParameters(parameters);
+	}
+	virtual ~CamPostTreatment() {}
+	virtual SMState * process(IplImage * image);
+	virtual void parseParameters(const ParametersMap & parameters) {}
+};
+
+/**
+ * Extract keypoints from the image
+ */
+class RTABMAP_EXP CamKeypointTreatment : public CamPostTreatment
+{
+public:
+	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
+	enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
+
+public:
+	CamKeypointTreatment(const ParametersMap & parameters = ParametersMap()) :
+		_keypointDetector(0),
+		_keypointDescriptor(0)
+	{
+		this->parseParameters(parameters);
+	}
+	virtual ~CamKeypointTreatment();
+	virtual SMState * process(IplImage * image);
+	virtual void parseParameters(const ParametersMap & parameters);
+	DetectorStrategy detectorStrategy() const;
+private:
+	KeypointDetector * _keypointDetector;
+	KeypointDescriptor * _keypointDescriptor;
+};
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_EXP Camera :
+	public UThreadNode,
+	public UEventsHandler
+{
+public:
+	enum State {kStateCapturing, kStateChangingParameters};
+
+public:
+	virtual ~Camera();
+
+	virtual IplImage * takeImage() = 0;
+	virtual bool init() = 0;
+	bool isPaused() const {return !this->isRunning();}
+	bool isCapturing() const {return this->isRunning();}
+	unsigned int getImageWidth() const {return _imageWidth;}
+	unsigned int getImageHeight() const {return _imageHeight;}
+	void setPostThreatement(CamPostTreatment * strategy); // ownership is transferred
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param imageRate : image/second , 0 for fast as the camera can
+	 */
+	Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0);
+
+	virtual void handleEvent(UEvent* anEvent);
+
+private:
+	virtual void mainLoop();
+	void process();
+	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	virtual void parseParameters(const ParametersMap & parameters) {_postThreatement->parseParameters(parameters);}
+
+private:
+	float _imageRate;
+	int _id;
+	bool _autoRestart;
+	unsigned int _imageWidth;
+	unsigned int _imageHeight;
+	CamPostTreatment * _postThreatement;
+
+	UMutex _stateMutex;
+	std::stack<State> _state;
+	std::stack<ParametersMap> _stateParam;
+};
+
+
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+class RTABMAP_EXP CameraImages :
+	public Camera
+{
+public:
+	CameraImages(const std::string & path,
+			int startAt = 1,
+			bool refreshDir = false,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+	virtual ~CameraImages();
+
+	virtual IplImage * takeImage();
+	virtual bool init();
+
+private:
+	std::string _path;
+	int _startAt;
+	// If the list of files in the directory is refreshed
+	// on each call of takeImage()
+	bool _refreshDir;
+	UDirectory * _dir;
+	int _count;
+	std::string _lastFileName;
+
+
+
+};
+
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+class RTABMAP_EXP CameraVideo :
+	public Camera
+{
+public:
+	enum Source{kVideoFile, kUsbDevice};
+
+public:
+	CameraVideo(int usbDevice = 0,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+	CameraVideo(const std::string & fileName,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+	virtual ~CameraVideo();
+
+	virtual IplImage * takeImage();
+	virtual bool init();
+
+private:
+	// File type
+	std::string _fileName;
+
+	CvCapture* _capture;
+	Source _src;
+
+	// Usb camera
+	int _usbDevice;
+};
+
+
+
+
+
+
+/////////////////////////
+// CameraDatabase
+/////////////////////////
+class DBDriver;
+class RTABMAP_EXP CameraDatabase :
+	public Camera
+{
+public:
+	CameraDatabase(const std::string & path,
+			bool ignoreChildren,
+			float imageRate = 0,
+			bool autoRestart = false,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0);
+	virtual ~CameraDatabase();
+
+	virtual IplImage * takeImage();
+	virtual bool init();
+
+private:
+	std::string _path;
+	bool _ignoreChildren;
+	std::set<int>::iterator _indexIter;
+	DBDriver * _dbDriver;
+	std::set<int> _ids;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraEvent.h

@@ -0,0 +1,79 @@
+/*
+ * 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 CAMERAEVENT_H_
+#define CAMERAEVENT_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "utilite/UEvent.h"
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP CameraEvent :
+	public UEvent
+{
+public:
+	enum Code {
+		kCodeCtrl,
+		kCodeNoMoreImages
+	};
+
+	enum Cmd {
+		kCmdUndefined,
+		kCmdPause,
+		kCmdChangeParam
+	};
+
+public:
+	CameraEvent(Cmd command, float imageRate = -1, bool autoRestart = false) :
+		UEvent(kCodeCtrl),
+		_command(command),
+		_imageRate(imageRate),
+		_autoRestart(autoRestart)
+	{
+	}
+
+	CameraEvent() :
+		UEvent(kCodeNoMoreImages),
+		_command(kCmdUndefined),
+		_imageRate(-1)
+	{
+
+	}
+
+	virtual ~CameraEvent() {}
+
+	virtual std::string getClassName() const {return std::string("CameraEvent");}
+
+	const Cmd & getCommand() const {return _command;}
+	float getImageRate() const {return _imageRate;}
+	bool getAutoRestart() const {return _autoRestart;}
+
+private:
+	Cmd _command;
+	float _imageRate;
+	bool _autoRestart;
+};
+
+} // namespace rtabmap
+
+#endif /* CAMERAEVENT_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -0,0 +1,178 @@
+/*
+ * 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 DBDRIVER_H_
+#define DBDRIVER_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <string>
+#include <list>
+#include <map>
+#include <set>
+#include <opencv2/core/core.hpp>
+#include "utilite/UMutex.h"
+#include "utilite/UThreadNode.h"
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap {
+
+class Signature;
+class KeypointSignature;
+class VWDictionary;
+class VisualWord;
+
+// Todo This class needs a refactoring, the _dbSafeAccessMutex problem when the trash is emptying (transaction)
+// "Of course, it has always been the case and probably always will be
+//that you cannot use the same sqlite3 connection in two or more
+//threads at the same time.  You can use different sqlite3 connections
+//at the same time in different threads, or you can move the same
+//sqlite3 connection across threads (subject to the constraints above)
+//but never, never try to use the same connection simultaneously in
+//two or more threads."
+//
+class RTABMAP_EXP DBDriver : public UThreadNode
+{
+public:
+	virtual ~DBDriver();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	const std::string & getUrl() const {return _url;}
+
+	void beginTransaction() const;
+	void commit() const;
+
+	void asyncSave(Signature * s);
+	void asyncSave(VisualWord * s);
+	void emptyTrashes(bool async = false);
+	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+
+public:
+	bool addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
+	bool addStatisticsAfterRunSurf(int dictionarySize) const;
+
+	bool deleteAllVisualWords() const;
+	bool deleteAllObsoleteSSVWLinks() const;
+	bool deleteUnreferencedWords() const;
+
+	bool addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates);
+	bool removeNeighbor(int id, int neighbor);
+
+public:
+	// Mutex-protected methods of abstract versions below
+	bool getSignature(int signatureId, Signature ** s);
+	bool getVisualWord(int wordId, VisualWord ** vw);
+
+	bool openConnection(const std::string & url);
+	void closeConnection();
+	bool isConnected() const;
+	long getMemoryUsed() const; // In bytes
+
+	bool executeNoResult(const std::string & sql) const;
+
+	// Update
+	bool changeWordsRef(const std::map<int, int> & refsToChange); // <oldWordId, activeWordId>
+	bool deleteWords(const std::vector<int> & ids);
+
+	// Load objects
+	bool load(VWDictionary * dictionary) const;
+	bool loadLastSignatures(std::list<Signature *> & signatures) const;
+	bool loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false);
+	bool loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws);
+
+	// Specific queries...
+	bool getImage(int id, IplImage ** img) const;
+	bool getNeighborIds(int signatureId, std::set<int> & neighbors) const;
+	bool loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const;
+	bool getWeight(int signatureId, int & weight) const;
+	bool getLoopClosureId(int signatureId, int & loopId) const;
+	bool getImageCompressed(int id, CvMat ** compressed) const;
+	bool getAllSignatureIds(std::set<int> & ids) const;
+	bool getLastSignatureId(int & id) const;
+	bool getLastVisualWordId(int & id) const;
+	bool getSurfNi(int signatureId, int & ni) const;
+	bool getChildrenIds(int signatureId, std::list<int> & ids) const;
+	bool getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const;
+
+protected:
+	DBDriver(const ParametersMap & parameters = ParametersMap());
+
+private:
+	virtual bool connectDatabaseQuery(const std::string & url) = 0;
+	virtual void disconnectDatabaseQuery() = 0;
+	virtual bool isConnectedQuery() const = 0;
+	virtual long getMemoryUsedQuery() const = 0; // In bytes
+
+	virtual bool executeNoResultQuery(const std::string & sql) const = 0;
+
+	virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const = 0; // <oldWordId, activeWordId>
+	virtual bool deleteWordsQuery(const std::vector<int> & ids) const = 0;
+	virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const = 0;
+	virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
+	virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const = 0;
+	virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const = 0;
+
+	virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const = 0;
+	virtual bool updateQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual bool saveQuery(const KeypointSignature * ss) const = 0;
+	virtual bool saveQuery(const std::list<KeypointSignature *> & signatures) const = 0;
+
+	// Load objects
+	virtual bool loadQuery(VWDictionary * dictionary) const = 0;
+	virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const = 0;
+	virtual bool loadQuery(int signatureId, Signature ** s) const = 0;
+	virtual bool loadQuery(int wordId, VisualWord ** vw) const = 0;
+	virtual bool loadQuery(int signatureId, KeypointSignature * ss) const = 0;
+	virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false) const = 0;
+	virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
+	virtual bool loadNeighborsQuery(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const = 0;
+
+	virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const = 0;
+	virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const = 0;
+	virtual bool getLastSignatureIdQuery(int & id) const = 0;
+	virtual bool getLastVisualWordIdQuery(int & id) const = 0;
+	virtual bool getSurfNiQuery(int signatureId, int & ni) const = 0;
+	virtual bool getChildrenIdsQuery(int signatureId, std::list<int> & ids) const = 0;
+	virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
+
+private:
+	//non-abstract methods
+	bool saveOrUpdate(const std::vector<Signature *> & signatures) const;
+
+	//thread stuff
+	virtual void mainLoop();
+	virtual void killCleanup();
+
+private:
+	UMutex _transactionMutex;
+	std::map<int, Signature *> _trashSignatures;//<id, Signature*>
+	std::map<int, VisualWord *> _trashVisualWords; //<id, VisualWord*>
+	UMutex _trashesMutex;
+	UMutex _dbSafeAccessMutex;
+	USemaphore _addSem;
+	unsigned int _minSignaturesToSave;
+	unsigned int _minWordsToSave;
+	bool _asyncWaiting;
+	double _emptyTrashesTime;
+	std::string _url;
+};
+
+}
+
+#endif /* DBDRIVER_H_ */

corelib/include/rtabmap/core/DBDriverFactory.h

@@ -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/>.
+ */
+
+#ifndef DBDRIVERFACTORY_H_
+#define DBDRIVERFACTORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/Parameters.h"
+#include <string>
+
+namespace rtabmap {
+
+class DBDriver;
+
+class RTABMAP_EXP DBDriverFactory
+{
+public:
+	static DBDriver * createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters = ParametersMap());
+public:
+	DBDriverFactory();
+	virtual ~DBDriverFactory();
+};
+
+}
+
+#endif /* DBDRIVERFACTORY_H_ */

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -0,0 +1,33 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+//epipolar geometry
+void RTABMAP_EXP findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2);
+void RTABMAP_EXP findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2 = cv::Vec3d());
+
+} // namespace rtabmap

corelib/include/rtabmap/core/KeypointDescriptor.h

@@ -0,0 +1,142 @@
+/*
+ * 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 KEYPOINTDESCRIPTOR_H_
+#define KEYPOINTDESCRIPTOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap {
+
+class RTABMAP_EXP KeypointDescriptor {
+public:
+	virtual ~KeypointDescriptor();
+
+	std::list<std::vector<float> > generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+	void setChildDescriptor(KeypointDescriptor * childDescriptor);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+protected:
+	KeypointDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	const KeypointDescriptor * getChildDescriptor() const {return _childDescriptor;}
+
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(
+		const IplImage * image,
+		const std::list<cv::KeyPoint> & keypoints) const = 0;
+
+private:
+	KeypointDescriptor * _childDescriptor;
+};
+
+//SURFDescriptor
+class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
+{
+public:
+	SURFDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	virtual ~SURFDescriptor();
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+
+private:
+	cv::SURF _surf;
+	bool _gpuVersion;
+	bool _upright;
+};
+
+//SIFTDescriptor
+class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
+{
+public:
+	SIFTDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	virtual ~SIFTDescriptor();
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+
+private:
+	cv::SIFT::CommonParams _commonParams;
+	cv::SIFT::DescriptorParams _descriptorParams;
+};
+
+//LaplacianDescriptor
+class RTABMAP_EXP LaplacianDescriptor : public KeypointDescriptor
+{
+public:
+	LaplacianDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	virtual ~LaplacianDescriptor();
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+};
+
+//MinMax ColorDescriptor
+class RTABMAP_EXP ColorDescriptor : public KeypointDescriptor
+{
+public:
+	ColorDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	virtual ~ColorDescriptor();
+	virtual void parseParameters(const ParametersMap & parameters);
+protected:
+	void getCircularROI(int R, std::vector<int> & RxV) const;
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+};
+
+//MinMax HueDescriptor
+class RTABMAP_EXP HueDescriptor : public ColorDescriptor
+{
+public:
+	HueDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	virtual ~HueDescriptor();
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+
+	// assuming that rgb values are normalized [0,1]
+	float rgb2hue(float r, float g, float b) const;
+
+	// assuming that rgb values are normalized [0,1]
+	inline float rgb2saturation(float r, float g, float b) const
+	{
+		float min = r;
+		min<g?min=g:min;
+		min<b?min=b:min;
+		float eps = 0.00001f;
+
+		return 1-(3*min)/(r+g+b+eps);
+	}
+
+	// assuming that rgb values are normalized [0,1]
+	inline float rgb2intensity(float r, float g, float b) const
+	{
+		return (r+g+b)/3;
+	}
+};
+
+}
+
+#endif /* KEYPOINTDESCRIPTOR_H_ */

corelib/include/rtabmap/core/KeypointDetector.h

@@ -0,0 +1,107 @@
+/*
+ * 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 KEYPOINTDETECTOR_H_
+#define KEYPOINTDETECTOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class VWDictionary;
+
+class RTABMAP_EXP KeypointDetector
+{
+public:
+	virtual ~KeypointDetector() {}
+	std::list<cv::KeyPoint> generateKeypoints(const IplImage * image);
+	virtual void parseParameters(const ParametersMap & parameters);
+	unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
+	double getAdaptiveResponseThr() const {return _adaptiveResponseThr;}
+	virtual double getMinimumResponseThr() const = 0;
+	bool isUsingAdaptiveResponseThr() const {return _usingAdaptiveResponseThr;}
+	void setRoi(const std::string & roi);
+protected:
+	KeypointDetector(const ParametersMap & parameters = ParametersMap());
+	void setAdaptiveResponseThr(float adaptiveResponseThr) {_adaptiveResponseThr = adaptiveResponseThr;}
+private:
+	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const = 0;
+	cv::Rect computeRoi(const IplImage * image) const;
+private:
+	unsigned int _wordsPerImageTarget;
+	bool _usingAdaptiveResponseThr;
+	double _adaptiveResponseThr;
+	std::vector<float> _roiRatios; // size 4
+};
+
+//SURFDetector
+class RTABMAP_EXP SURFDetector : public KeypointDetector
+{
+public:
+	SURFDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURFDetector();
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual double getMinimumResponseThr() const {return _surf.hessianThreshold;};
+private:
+	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+private:
+	cv::SURF _surf;
+	bool _gpuVersion;
+	bool _upright;
+};
+
+//SIFTDetector
+class RTABMAP_EXP SIFTDetector : public KeypointDetector
+{
+public:
+	SIFTDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~SIFTDetector();
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual double getMinimumResponseThr() const {return _detectorParams.threshold;};
+private:
+	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+private:
+	cv::SIFT::CommonParams _commonParams;
+	cv::SIFT::DetectorParams _detectorParams;
+};
+
+//StarDetector
+class RTABMAP_EXP StarDetector : public KeypointDetector
+{
+public:
+	StarDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~StarDetector();
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual double getMinimumResponseThr() const {return (double)_star.responseThreshold;};
+private:
+	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+private:
+	cv::StarDetector _star;
+};
+
+}
+
+#endif /* KEYPOINTDETECTOR_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -0,0 +1,243 @@
+/*
+ * 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 PARAMETERS_H_
+#define PARAMETERS_H_
+
+// default parameters
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "utilite/UEvent.h"
+#include <string>
+#include <map>
+#include "utilite/UDestroyer.h"
+
+namespace rtabmap
+{
+
+typedef std::map<std::string, std::string> ParametersMap; // Key, value
+typedef std::pair<const std::string, std::string> ParametersPair;
+
+/**
+ * Macro used to create parameter's key and default value.
+ * This macro must be used only in the Parameters class definition (in this file).
+ * They are automatically added to the default parameters map of the class Parameters.
+ * Example:
+ * @code
+ * 		//for PARAM(Video, ImageWidth, int, 640), the output will be :
+ * 		public:
+ * 			static std::string kVideoImageWidth() {return std::string("Video/ImageWidth");}
+ * 			static int defaultVideoImageWidth() {return 640;}
+ * 		private:
+ * 			class DummyVideoImageWidth {
+ * 			public:
+ * 				DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
+ * 			};
+ * 			DummyVideoImageWidth dummyVideoImageWidth;
+ * @endcode
+ */
+#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) \
+	public: \
+		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+		static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+	private: \
+		class Dummy##PREFIX##NAME { \
+		public: \
+			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE));} \
+		}; \
+		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+// end define PARAM
+
+/**
+ * It's the same as the macro PARAM but it should be used for string parameters.
+ * Macro used to create parameter's key and default value.
+ * This macro must be used only in the Parameters class definition (in this file).
+ * They are automatically added to the default parameters map of the class Parameters.
+ * Example:
+ * @code
+ * 		//for PARAM_STR(Video, TextFileName, "Hello_world"), the output will be :
+ * 		public:
+ * 			static std::string kVideoFileName() {return std::string("Video/FileName");}
+ * 			static std::string defaultVideoFileName() {return "Hello_world";}
+ * 		private:
+ * 			class DummyVideoFileName {
+ * 			public:
+ * 				DummyVideoFileName() {parameters_.insert(ParametersPair("Video/FileName", "Hello_world"));}
+ * 			};
+ * 			DummyVideoFileName dummyVideoFileName;
+ * @endcode
+ */
+#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE) \
+	public: \
+		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+		static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+	private: \
+		class Dummy##PREFIX##NAME { \
+		public: \
+			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE));} \
+		}; \
+		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+// end define PARAM
+
+/**
+ * Class Parameters.
+ * This class is used to manage all custom parameters
+ * we want in the application. It was designed to be very easy to add
+ * a new parameter (just by adding one line of code).
+ * The macro PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) is
+ * used to create a parameter in this class. A parameter can be accessed after by
+ * Parameters::defaultPARAMETERNAME() for the default value, Parameters::kPARAMETERNAME for his key (parameter name).
+ * The class provides also a general map containing all the parameter's key and
+ * default value. This map can be accessed anywhere in the application by
+ * Parameters::getDefaultParameters();
+ * Example:
+ * @code
+ * 		//Defining a parameter in this class with the macro PARAM:
+ * 		PARAM(Video, ImageWidth, int, 640);
+ *
+ * 		// Now from anywhere in the application (Parameters is a singleton)
+ * 		int width = Parameters::defaultVideoImageWidth(); // theDefaultValue = 640
+ * 		std::string theKey = Parameters::kVideoImageWidth(); // theKey = "Video/ImageWidth"
+ * 		std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
+ * @endcode
+ * @see getDefaultParameters()
+ * TODO Add a detailed example with simple classes
+ */
+class RTABMAP_EXP Parameters
+{
+	// Rtabmap parameters
+	RTABMAP_PARAM(Rtabmap, VhStrategy, 	        int, 0);	   // Simple 0, Epipolar 1
+	RTABMAP_PARAM(Rtabmap, PublishStats, 	     bool, true); // Publishing statistics
+	RTABMAP_PARAM(Rtabmap, ReactivationThr,     float, 0.0); // Reactivation threshold
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		     float, 0.7); // Maximum time allowed for the detector (s) (0 means infinity)
+	RTABMAP_PARAM(Rtabmap, DisableReactivation, bool, false); // Memory reactivation when a loop closure occurs : 0=enable, 1=disable
+	RTABMAP_PARAM(Rtabmap, SMStateBufferSize,    int, 1);      // Data buffer size (0 min inf)
+	RTABMAP_PARAM(Rtabmap, MinMemorySizeForLoopDetection, unsigned int, 15); //Minimum size of the memory to create loop closure hypotheses
+	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory());	   // Working directory
+	RTABMAP_PARAM(Rtabmap, LocalGraphCleaned, bool, false);	   // Clean the neighborhood of the retrieved id
+	RTABMAP_PARAM(Rtabmap, MaxRetrieved,       unsigned int, 2); // Maximum locations retrieved at the same time from LTM
+
+	// Hypotheses selection
+	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.10); // Loop closing threshold
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.90); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
+
+	// Memory
+	RTABMAP_PARAM(Mem, SimilarityThr, 	       float, 0.20);  // Similarity between the last signature and neighbor
+	RTABMAP_PARAM(Mem, SimilarityOnlyLast,     bool, false);	// Only compare to the last signature in STM, otherwise it compares to all signatures in STM
+	RTABMAP_PARAM(Mem, RawDataKept, 		   bool, false); // Keep raw data
+	RTABMAP_PARAM(Mem, MaxStMemSize, 		   unsigned int, 25); // Short-time memory size
+	RTABMAP_PARAM(Mem, CommonSignatureUsed,    bool, true); // A common signature/virtual place is automatically updated with id -1
+	RTABMAP_PARAM(Mem, IncrementalMemory, 	   bool, true);
+	RTABMAP_PARAM(Mem, DatabaseCleaned, 	   bool, true);  // Delete old signatures in the database (the ones which can't never be reactivated)
+	RTABMAP_PARAM(Mem, DelayRequired, 	       int, 10);     // Delay (in iterations) required to transfer signatures
+	RTABMAP_PARAM(Mem, RecentWmRatio,          float, 0.2);  // Ratio of locations after the last loop closure in WM that cannot be transferred
+
+	// KeypointMemory (Keypoint-based)
+	RTABMAP_PARAM(Kp, NNStrategy,     	int, 2);	  // Naive 0, kdTree 1, kdForest 2
+	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
+	RTABMAP_PARAM(Kp, WordsPerImage,    int, 400);
+	RTABMAP_PARAM(Kp, BadSignRatio,    float, 0.05); //Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)
+	RTABMAP_PARAM(Kp, MinDistUsed,     bool, false); // The nearest neighbor must have a distance < minDist
+	RTABMAP_PARAM(Kp, MinDist, 	    float, 0.05); // Matching a descriptor with a word (euclidean distance ^ 2)
+	RTABMAP_PARAM(Kp, NndrUsed, 	    bool, true);  // If NNDR ratio is used
+	RTABMAP_PARAM(Kp, NndrRatio, 	    float, 0.8);  // NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)
+	RTABMAP_PARAM(Kp, MaxLeafs, 	    int, 64);  // Maximum number of leafs checked (when using kd-trees)
+	RTABMAP_PARAM(Kp, DetectorStrategy, int, 0);      // Surf detector 0, Star detector 1
+	RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0);      // kDescriptorSurf=0, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef
+	RTABMAP_PARAM(Kp, UsingAdaptiveResponseThr, bool, false);
+	RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
+	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
+	RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
+	RTABMAP_PARAM(Kp, TfIdfNormalized, bool, false); // If tf-idf weighting is normalized by the words count ratio between compared signatures
+	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
+	RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary
+
+	//Database
+	RTABMAP_PARAM(Db, MinSignaturesToSave, int, 20);	 // Minimum signatures needed in the trash to save them (empty trash thread)
+	RTABMAP_PARAM(Db, MinWordsToSave, 	   int, 4000);	 // Minimum visual words needed in the trash to save them (empty trash thread)
+	RTABMAP_PARAM(DbSqlite3, InMemory, 	   bool, false);	 // Using database in the memory instead of a file on the hard disk
+	RTABMAP_PARAM(DbSqlite3, CacheSize,    unsigned int, 2000);	 // Sqlite cache size (default is 2000)
+	RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 0);	 	 // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
+
+	RTABMAP_PARAM(SURF, Extended, 		  bool, false); // true=128, false=64
+	RTABMAP_PARAM(SURF, HessianThreshold, float, 100.0);
+	RTABMAP_PARAM(SURF, Octaves, 		  int, 4);
+	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2);
+	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false);
+	RTABMAP_PARAM(SURF, Upright, 	      bool, false); // U-SURF
+
+	RTABMAP_PARAM(SIFT, Threshold, 	double, 0.006667); // true=128, false=64
+	RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
+
+	RTABMAP_PARAM(Star, MaxSize, int, 45);
+	RTABMAP_PARAM(Star, ResponseThreshold, int, 30);
+	RTABMAP_PARAM(Star, LineThresholdProjected, int, 10);
+	RTABMAP_PARAM(Star, LineThresholdBinarized, int, 8);
+	RTABMAP_PARAM(Star, SuppressNonmaxSize, int, 5);
+
+	// BayesFilter
+	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9);	// Virtual place prior
+	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.24 0.18 0.1 0.04 0.01"); // Prediction of loop closures (Gaussian-like, must be pair size) - Format: {VirtualPlaceProb, LoopClosureProb, BackwardNeighborLvl1, ForwardNeighborLvl1, BackwardNeighborLvl2, ForwardNeighborLvl2, ...}
+
+	// Verify hypotheses
+	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8);     // Minimum of matching visual words pairs to accept the loop hypothesis
+	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0);  // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
+	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC
+
+public:
+	virtual ~Parameters();
+	static const ParametersMap & getDefaultParameters();
+
+private:
+	Parameters();
+	static Parameters * getInstance();
+	const ParametersMap & getParameters() const;
+	void addParameter(const std::string & key, const std::string & value);
+	static std::string getDefaultWorkingDirectory();
+
+private:
+	static Parameters * instance_;
+	static UDestroyer<Parameters> destroyer_;
+	static ParametersMap parameters_;
+};
+
+/**
+ * The parameters event. This event is used to send
+ * parameters across the threads.
+ */
+class ParamEvent : public UEvent
+{
+public:
+	ParamEvent(const ParametersMap & parameters) : UEvent(0), parameters_(parameters) {}
+	ParamEvent(const std::string & parameterKey, const std::string & parameterValue) : UEvent(0)
+	{
+		parameters_.insert(std::pair<std::string, std::string>(parameterKey, parameterValue));
+	}
+	~ParamEvent() {}
+	virtual std::string getClassName() const {return "ParamEvent";}
+
+	const ParametersMap & getParameters() const {return parameters_;}
+
+private:
+	ParametersMap parameters_; /**< The parameters map (key,value). */
+};
+
+}
+
+#endif /* PARAMETERS_H_ */
+

corelib/include/rtabmap/core/Rtabmap.h

@@ -0,0 +1,165 @@
+/*
+ * 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 CTABMAP_H_
+#define CTABMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "utilite/UThreadNode.h"
+#include "utilite/UEventsHandler.h"
+#include "utilite/USemaphore.h"
+#include "utilite/UMutex.h"
+#include "utilite/UVariant.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/Parameters.h"
+#include <opencv2/core/core.hpp>
+#include <list>
+#include <stack>
+#include <set>
+
+namespace rtabmap
+{
+
+class Signature;
+
+class VerifyHypotheses;
+class Memory;
+class BayesFilter;
+class SMState;
+
+class RTABMAP_EXP Rtabmap :
+	public UThreadNode,
+	public UEventsHandler
+{
+public:
+	enum State {
+		kStateIdle,
+		kStateDetecting,
+		kStateReseting,
+		kStateChangingParameters,
+		kStateDumpingMemory,
+		kStateDumpingPrediction,
+		kStateGeneratingGraph,
+		kStateDeletingMemory
+	};
+
+	enum VhStrategy {kVhSimple, kVhEpipolar, kVhUndef};
+
+	static const char * kDefaultIniFileName;
+	static const char * kDefaultIniFilePath;
+
+public:
+	static std::string getVersion();
+	static std::string getIniFilePath();
+	static void readParameters(const char * configFile, ParametersMap & parameters);
+	static void writeParameters(const char * configFile, const ParametersMap & parameters);
+
+public:
+	Rtabmap();
+	virtual ~Rtabmap();
+
+	void process(SMState * data);
+	void dumpData();
+
+	void init(const ParametersMap & param);
+	void init(const char * configFile = 0);
+
+	const std::string & getWorkingDir() const {return _wDir;}
+	int getLoopClosureId() const;
+	int getLastSignatureId() const;
+	const std::list<std::vector<float> > & getActions() const {return _actions;}
+	std::list<int> getWorkingMem() const;
+	std::set<int> getStMem() const;
+	std::map<int, int> getWeights() const;
+	int getTotalMemSize() const;
+	const std::string & getGraphFileName() const {return _graphFileName;}
+
+	void setReactivationDisabled(bool reactivationDisabled);
+	void setMaxTimeAllowed(float maxTimeAllowed); // in sec
+	void setDataBufferSize(int size);
+	void setWorkingDirectory(std::string path);
+	void setGraphFileName(const std::string & fileName) {_graphFileName = fileName;}
+
+	void adjustLikelihood(std::map<int, float> & likelihood) const;
+	void selectHypotheses(const std::map<int, float> & posterior,
+						  std::list<std::pair<int, float> > & hypotheses,
+						  bool useNeighborSum) const;
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	virtual void mainLoop();
+	virtual void killCleanup();
+	virtual void startInit();
+	void process();
+	void addSMState(SMState * data);
+	SMState * getSMState();
+	void setupLogFiles();
+	void releaseAllStrategies();
+	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
+	void dumpPrediction() const;
+	void parseParameters(const ParametersMap & parameters);
+
+private:
+	// Modifiable parameters
+	bool _publishStats;
+	bool _reactivationDisabled;
+	float _maxTimeAllowed; // in sec
+	int _smStateBufferMaxSize;
+	unsigned int _minMemorySizeForLoopDetection;
+	float _loopThr;
+	float _loopRatio;
+	float _remThr;
+	bool _localGraphCleaned;
+	unsigned int _maxRetrieved;
+
+	int _lcHypothesisId;
+	int _reactivateId;
+	float _highestHypothesisValue;
+	unsigned int _spreadMargin;
+	int _lastLoopClosureId;
+	std::list<std::vector<float> > _actions;
+
+	UMutex _stateMutex;
+	std::stack<State> _state;
+	std::stack<ParametersMap> _stateParam;
+
+	std::list<SMState *> _smStateBuffer;
+	UMutex _smStateBufferMutex;
+	USemaphore _newSMStateSem;
+
+	// Abstract classes containing all loop closure
+	// strategies for a type of signature or configuration.
+	VerifyHypotheses * _vhStrategy;
+	BayesFilter * _bayesFilter;
+
+	Memory * _memory;
+
+	FILE* _foutFloat;
+	FILE* _foutInt;
+
+	std::string _wDir;
+	std::string _graphFileName;
+};
+
+#endif /* CTABMAP_H_ */
+
+} // namespace rtabmap

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -0,0 +1,233 @@
+/*
+ * 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 RTABMAPEVENT_H_
+#define RTABMAPEVENT_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "utilite/UEvent.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include "utilite/ULogger.h"
+#include <list>
+#include <vector>
+
+namespace rtabmap
+{
+
+#define RTABMAP_STATS(PREFIX, NAME, UNIT) \
+	public: \
+		static std::string k##PREFIX##NAME() {return #PREFIX "/" #NAME "/" #UNIT;} \
+	private: \
+		class Dummy##PREFIX##NAME { \
+		public: \
+			Dummy##PREFIX##NAME() {if(!_defaultDataInitialized)_defaultData.insert(std::pair<std::string, float>(#PREFIX "/" #NAME "/" #UNIT, 0.0f));} \
+		}; \
+		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
+
+class RTABMAP_EXP Statistics
+{
+	RTABMAP_STATS(Loop, Closure_id,);
+	RTABMAP_STATS(Loop, Rejected_reason,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
+	RTABMAP_STATS(Loop, Vp_likelihood,);
+	RTABMAP_STATS(Loop, ReactivateId,);
+	RTABMAP_STATS(Loop, Hypothesis_ratio,);
+
+	RTABMAP_STATS(Memory, Working_memory_size,);
+	RTABMAP_STATS(Memory, Short_time_memory_size,);
+	RTABMAP_STATS(Memory, Database_size, MB);
+	RTABMAP_STATS(Memory, Process_memory_used, MB);
+	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Signatures_reactivated,);
+	RTABMAP_STATS(Memory, Images_buffered,);
+
+	RTABMAP_STATS(Timing, Memory_update, ms);
+	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
+	RTABMAP_STATS(Timing, Reactivation, ms);
+	RTABMAP_STATS(Timing, Likelihood_computation, ms);
+	RTABMAP_STATS(Timing, Posterior_computation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_validation, ms);
+	RTABMAP_STATS(Timing, Statistics_creation, ms);
+	RTABMAP_STATS(Timing, Memory_cleanup, ms);
+	RTABMAP_STATS(Timing, Total, ms);
+	RTABMAP_STATS(Timing, Forgetting, ms);
+	RTABMAP_STATS(Timing, Emptying_memory_trash, ms);
+
+	RTABMAP_STATS(, Parent_id,);
+	RTABMAP_STATS(, Hypothesis_reactivated,);
+
+	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Response_threshold,);
+
+public:
+	static const std::map<std::string, float> & defaultData();
+
+public:
+	Statistics();
+	Statistics(const Statistics & s);
+	virtual ~Statistics();
+
+	// name format = "Grp/Name/unit"
+	void addStatistic(const std::string & name, float value);
+
+	// setters
+	void setExtended(bool extended) {_extended = extended;}
+	void setRefImageId(int refImageId) {_refImageId = refImageId;}
+	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setActions(const std::list<std::vector<float> > & actions) {_actions = actions;}
+	void setRefImage(IplImage ** refImage);
+	void setRefImage(const IplImage * refImage);
+	void setLoopClosureImage(IplImage ** loopClosureImage);
+	void setLoopClosureImage(const IplImage * loopClosureImage);
+	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
+	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
+	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
+	void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
+	void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
+
+	// getters
+	bool extended() const {return _extended;}
+	int refImageId() const {return _refImageId;}
+	int loopClosureId() const {return _loopClosureId;}
+	const std::list<std::vector<float> > & getActions() const {return _actions;}
+	const IplImage * refImage() const {return _refImage;}
+	const IplImage * loopClosureImage() const {return _loopClosureImage;}
+	const std::map<int, int> & weights() const {return _weights;}
+	const std::map<int, float> & posterior() const {return _posterior;}
+	const std::map<int, float> & likelihood() const {return _likelihood;}
+	const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
+	const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
+	const std::map<std::string, float> & data() const {return _data;}
+
+
+	Statistics & operator=(const Statistics & s);
+
+private:
+	int _extended; // 0 -> only loop closure and last signature ID fields are filled
+
+	int _refImageId;
+	int _loopClosureId;
+
+	std::list<std::vector<float> > _actions;
+
+	// extended data start here...
+	IplImage * _refImage; // Released by the event destructor
+	IplImage * _loopClosureImage; // Released by the event destructor
+
+	std::map<int, int> _weights;
+	std::map<int, float> _posterior;
+	std::map<int, float> _likelihood;
+
+	//surf
+	std::multimap<int, cv::KeyPoint> _refWords;
+	std::multimap<int, cv::KeyPoint> _loopWords;
+
+	// Format for statistics (Plottable statistics must go in that map) :
+	// {"Group/Name/Unit", value}
+	// Example : {"Timing/Total time/ms", 500.0f}
+	std::map<std::string, float> _data;
+	static std::map<std::string, float> _defaultData;
+	static bool _defaultDataInitialized;
+	// end extended data
+};
+
+
+////////// The RtabmapEvent class //////////////
+class RtabmapEvent : public UEvent
+{
+public:
+	RtabmapEvent(Statistics ** stats) :
+		UEvent(0),
+		_stats(*stats) {}
+
+	virtual ~RtabmapEvent() {if(_stats) delete _stats;}
+	const Statistics & getStats() const {return *_stats;}
+	virtual std::string getClassName() const {return std::string("RtabmapEvent");}
+
+private:
+	Statistics * _stats;
+};
+
+class RtabmapEventCmd : public UEvent
+{
+public:
+	enum Cmd {
+		kCmdResetMemory,
+		kCmdDumpMemory,
+		kCmdDumpPrediction,
+		kCmdGenerateGraph,
+		kCmdDeleteMemory};
+public:
+	RtabmapEventCmd(Cmd cmd) :
+		UEvent(0),
+		_cmd(cmd) {}
+
+	virtual ~RtabmapEventCmd() {}
+	Cmd getCmd() const {return _cmd;}
+	void setStr(const std::string & str) {_str = str;}
+	const std::string & getStr() const {return _str;}
+	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
+
+private:
+	Cmd _cmd;
+	std::string _str;
+};
+
+class RtabmapEventInit : public UEvent
+{
+public:
+	enum Status {
+		kInitializing,
+		kInitialized,
+		kInfo,
+		kError
+	};
+
+public:
+	RtabmapEventInit(Status status, const std::string & info = std::string()) :
+		UEvent(0),
+		_status(status),
+		_info(info)
+	{}
+
+	// for convenience
+	RtabmapEventInit(const std::string & info) :
+		UEvent(0),
+		_status(kInfo),
+		_info(info)
+	{}
+
+	Status getStatus() const {return _status;}
+	const std::string & getInfo() const {return _info;}
+
+	virtual ~RtabmapEventInit() {}
+	virtual std::string getClassName() const {return std::string("RtabmapEventInit");}
+private:
+	Status _status;
+	std::string _info; // "Loading signatures", "Loading words" ...
+};
+
+} // namespace rtabmap
+
+#endif /* RTABMAPEVENT_H_ */

corelib/include/rtabmap/core/RtabmapExp.h

@@ -0,0 +1,37 @@
+/*
+ * 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 RTABMAPEXP_H
+#define RTABMAPEXP_H
+
+#ifdef WIN32
+  #ifdef RTABMAP_EXPORTS
+    #define RTABMAP_EXP   __declspec( dllexport )
+  #else
+    #ifdef RTABMAP_EXPORTS_STATIC
+    #define RTABMAP_EXP
+    #else
+      #define RTABMAP_EXP   __declspec( dllimport )
+    #endif
+  #endif
+#else
+  #define RTABMAP_EXP
+#endif
+
+#endif // RTABMAPEXP_H

corelib/include/rtabmap/core/SMState.h

@@ -0,0 +1,97 @@
+/*
+ * 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 SMPAIRVARIANT_H_
+#define SMPAIRVARIANT_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include <vector>
+
+namespace rtabmap {
+
+// SensoriMotor state
+class SMState
+{
+public:
+	// Constructor 1
+	// image and/or keypoints can be passed for debugging (rtabmap will not re-extract keypoints/descriptors from the image if not null, only for debug/visualization)
+	// take image ownership
+	SMState(const std::list<std::vector<float> > & sensorStates, const std::list<std::vector<float> > & actuatorStates, IplImage * image = 0, const std::list<cv::KeyPoint> & keypoints = std::list<cv::KeyPoint>()) :
+		_sensorStates(sensorStates),
+		_actuatorStates(actuatorStates),
+		_image(image),
+		_keypoints(keypoints),
+		_descriptorsProvided(true)
+	{}
+	// Constructor 2 :
+	// rtabmap will automatically extract keypoints and descriptors from the image...
+	// take image ownership
+	SMState(IplImage * image, const std::list<std::vector<float> > & actuatorStates = std::list<std::vector<float> >()) :
+		_actuatorStates(actuatorStates),
+		_image(image),
+		_descriptorsProvided(false)
+	{}
+	virtual ~SMState()
+	{
+		if(_image)
+			cvReleaseImage(&_image);
+	}
+
+	bool isDescriptorsProvided() const {return _descriptorsProvided;}
+	const IplImage * getImage() const {return _image;}
+	const std::list<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
+	const std::list<std::vector<float> > & getSensorStates() const {return _sensorStates;}
+	const std::list<std::vector<float> > & getActuatorStates() const {return _actuatorStates;}
+
+	void setSensorStates(const std::list<std::vector<float> > & sensorStates) {_sensorStates=sensorStates;}
+	void setActuatorStates(const std::list<std::vector<float> > & actuatorStates) {_actuatorStates=actuatorStates;}
+
+private:
+	std::list<std::vector<float> > _sensorStates; // descriptors
+	std::list<std::vector<float> > _actuatorStates;
+	IplImage * _image;
+	std::list<cv::KeyPoint> _keypoints;
+	bool _descriptorsProvided;
+};
+
+// Sensorimotor state event
+// Take ownership of the state
+class SMStateEvent : public UEvent
+{
+public:
+	SMStateEvent(SMState * state) :
+		UEvent(0),
+		_state(state) {}
+
+	virtual ~SMStateEvent() {if(_state) delete _state;}
+	const SMState * getData() const {return _state;}
+	SMState * getDataOwnership() {SMState * state = _state; _state=0; return state;}
+	virtual std::string getClassName() const {return "SMStateEvent";} // TODO : macro?
+
+private:
+	SMState * _state;
+};
+
+}
+
+#endif /* SMPAIRVARIANT_H_ */

corelib/src/BayesFilter.cpp

@@ -0,0 +1,462 @@
+/*
+ * 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 "BayesFilter.h"
+#include "Memory.h"
+#include "Signature.h"
+#include "rtabmap/core/Parameters.h"
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap {
+
+BayesFilter::BayesFilter(const ParametersMap & parameters) :
+	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr())
+{
+	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
+	this->parseParameters(parameters);
+}
+
+BayesFilter::~BayesFilter() {
+}
+
+void BayesFilter::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kBayesVirtualPlacePriorThr())) != parameters.end())
+	{
+		this->setVirtualPlacePrior(std::atof((*iter).second.c_str()));
+	}
+	if((iter=parameters.find(Parameters::kBayesPredictionLC())) != parameters.end())
+	{
+		this->setPredictionLC((*iter).second);
+	}
+}
+
+void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
+{
+	if(virtualPlacePrior < 0)
+	{
+		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
+		_virtualPlacePrior = 0;
+	}
+	else if(virtualPlacePrior > 1)
+	{
+		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
+		_virtualPlacePrior = 1;
+	}
+	else
+	{
+		_virtualPlacePrior = virtualPlacePrior;
+	}
+}
+
+// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
+void BayesFilter::setPredictionLC(const std::string & prediction)
+{
+	std::list<std::string> strValues = uSplit(prediction, ' ');
+	if(strValues.size() < 2)
+	{
+		ULOGGER_ERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
+	}
+	else
+	{
+		std::vector<double> tmpValues(strValues.size());
+		int i=0;
+		bool valid = true;
+		float sum = 0;;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			tmpValues[i] = std::atof((*iter).c_str());
+			sum += tmpValues[i];
+			if(i>1)
+			{
+				sum += tmpValues[i]; // add a second time
+			}
+			if(tmpValues[i] < 0 || tmpValues[i]>1)
+			{
+				valid = false;
+				break;
+			}
+			++i;
+		}
+
+		if(!valid || sum <= 0 || sum > 1.001)
+		{
+			ULOGGER_ERROR("The prediction is not valid (the sum must be between >0 && <=1, sum=%f), negative values are not allowed (prediction=\"%s\")", sum, prediction.c_str());
+		}
+		else
+		{
+			_predictionLC = tmpValues;
+		}
+	}
+}
+
+const std::vector<double> & BayesFilter::getPredictionLC() const
+{
+	// {Vp, Lc, l1, l2, l3, l4...}
+	return _predictionLC;
+}
+
+std::string BayesFilter::getPredictionLCStr() const
+{
+	std::string values;
+	for(unsigned int i=0; i<_predictionLC.size(); ++i)
+	{
+		values.append(uNumber2str(_predictionLC[i]));
+		if(i+1 < _predictionLC.size())
+		{
+			values.append(" ");
+		}
+	}
+	return values;
+}
+
+void BayesFilter::reset()
+{
+	_posterior.clear();
+}
+
+const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
+{
+	ULOGGER_DEBUG("");
+
+	if(!memory)
+	{
+		ULOGGER_ERROR("Memory is Null!");
+		return _posterior;
+	}
+
+	if(!likelihood.size())
+	{
+		ULOGGER_ERROR("likelihood is empty!");
+		return _posterior;
+	}
+
+	if(_predictionLC.size() < 2)
+	{
+		ULOGGER_ERROR("Prediction is not valid!");
+		return _posterior;
+	}
+
+	UTimer timer;
+	timer.start();
+
+	CvMat * prediction = 0;
+	CvMat * prior = 0;
+	CvMat * posterior = 0;
+
+	float sum = 0;
+	int j=0;
+	// Recursive Bayes estimation...
+	// STEP 1 - Prediction : Prior*lastPosterior
+	prediction = cvCreateMat(likelihood.size(), likelihood.size(), CV_32FC1);
+	std::map<int, int> likelihoodKeys;
+	int index = 0;
+	for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
+	{
+		likelihoodKeys.insert(likelihoodKeys.end(), std::pair<int, int>(iter->first, index++));
+	}
+
+	if(this->generatePrediction(prediction, memory, likelihoodKeys))
+	{
+		ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction->rows, prediction->cols);
+
+		// Adjust the last posterior if some images were
+		// reactivated or removed from the working memory
+		posterior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
+		this->updatePosterior(memory, uKeys(likelihood));
+		j=0;
+		for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
+		{
+			posterior->data.fl[j++] = (*i).second;
+		}
+		ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior->rows, _posterior.size());
+
+		// Multiply prediction matrix with the last posterior
+		// (m,m) X (m,1) = (m,1)
+		prior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
+		cvMatMul(prediction, posterior, prior);
+		ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
+
+		// STEP 2 - Update : Multiply with observations (likelihood)
+		j=0;
+		for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
+		{
+			std::map<int, float>::iterator p =_posterior.find((*i).first);
+			if(p!= _posterior.end())
+			{
+				(*p).second = (*i).second * prior->data.fl[j++];
+				sum+=(*p).second;
+			}
+			else
+			{
+				ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
+			}
+		}
+		ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
+
+		// Normalize
+		ULOGGER_DEBUG("sum=%f", sum);
+		if(sum != 0)
+		{
+			for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
+			{
+				(*i).second /= sum;
+			}
+		}
+		ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
+	}
+
+	cvReleaseMat(&prediction);
+	cvReleaseMat(&prior);
+	cvReleaseMat(&posterior);
+
+	return _posterior;
+}
+
+bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const
+{
+	ULOGGER_DEBUG("");
+	UTimer timer;
+	timer.start();
+	UTimer timerGlobal;
+	timerGlobal.start();
+
+	if(!likelihoodIds.size() ||
+	   prediction == 0 ||
+	   prediction->rows != prediction->cols ||
+	   (unsigned int)prediction->rows != likelihoodIds.size()/*||
+	   prediction->type != CV_32FC1*/ ||
+	   _predictionLC.size() < 2 ||
+	   !memory)
+	{
+		ULOGGER_ERROR( "fail");
+		return false;
+	}
+
+	//int rows = prediction->rows;
+	cvSetZero(prediction);
+	int cols = prediction->cols;
+
+	// Each priors are column vectors
+	unsigned int i=0;
+	ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
+	for(std::map<int, int>::const_iterator iter=likelihoodIds.begin(); iter!=likelihoodIds.end(); ++iter)
+	{
+		if(iter->first > 0)
+		{
+			// Create the sum of 2 gaussians around the loop closure
+
+			int loopClosureId = iter->first;
+
+			// Set high values (gaussians curves) to loop closure neighbors
+			const Signature * loopSign = memory->getSignature(loopClosureId);
+			if(!loopSign)
+			{
+				ULOGGER_ERROR("loopSign %d is not found?!?", loopClosureId);
+			}
+
+			// LoopID
+			prediction->data.fl[i + i*cols] += _predictionLC[1];
+
+			// look up for each neighbors (RECURSIVE)
+			this->addNeighborProb(prediction, i, memory, likelihoodIds, loopSign, 1);
+
+			//ULOGGER_DEBUG("neighbor prob for %d, neighbors=%d, time = %fs", loopSign->id(), loopSign->getNeighborIds().size(), timer.ticks());
+
+			float totalModelValues = _predictionLC[0] + _predictionLC[1];
+			for(unsigned int j=2; j<_predictionLC.size(); ++j)
+			{
+				totalModelValues += _predictionLC[j]*2;
+			}
+
+			//Add values of not found neighbors to the loop closure
+			float sum = 0;
+			for(int j=0; j<cols; ++j)
+			{
+				sum += prediction->data.fl[i + j*cols];
+			}
+			if(sum < (totalModelValues-_predictionLC[0]))
+			{
+				float gap = (totalModelValues-_predictionLC[0]) - sum;
+				prediction->data.fl[i + i*cols] += gap;
+				sum += gap;
+			}
+
+			// add virtual place prob
+			if(likelihoodIds.begin()->first < 0)
+			{
+				sum += prediction->data.fl[i] = _predictionLC[0];
+			}
+
+			// Set all loop events to small values according to the model
+			if(totalModelValues < 1.0f)
+			{
+				float value = (1.0f-totalModelValues) / float(cols);
+				for(int j=0; j<cols; ++j)
+				{
+					if(!prediction->data.fl[i + j*cols])
+					{
+						sum += prediction->data.fl[i + j*cols] = value;
+					}
+				}
+			}
+
+			//normalize this row,
+			for(int j=0; j<cols; ++j)
+			{
+				prediction->data.fl[i + j*cols] /= sum;
+			}
+
+			//debug
+			//for(int j=0; j<cols; ++j)
+			//{
+			//	ULOGGER_DEBUG("test = %f", prediction->data.fl[i + j*cols]);
+			//}
+		}
+		else
+		{
+			// Set the virtual place prior
+			if(_virtualPlacePrior > 0)
+			{
+				if(cols>1) // The first must be the virtual place
+				{
+					prediction->data.fl[i] = _virtualPlacePrior;
+					float val = (1.0-_virtualPlacePrior)/(cols-1);
+					for(int j=1; j<cols; j++)
+					{
+						prediction->data.fl[i + j*cols] = val;
+					}
+				}
+				else if(cols>0)
+				{
+					prediction->data.fl[i] = 1;
+				}
+			}
+			else
+			{
+				// Only for some tests...
+				// when _virtualPlacePrior=0, set all priors to the same value
+				if(cols>1)
+				{
+					float val = 1.0/cols;
+					for(int j=0; j<cols; j++)
+					{
+						prediction->data.fl[i + j*cols] = val;
+					}
+				}
+				else if(cols>0)
+				{
+					prediction->data.fl[i] = 1;
+				}
+			}
+		}
+		++i;
+	}
+
+	ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
+
+	return true;
+}
+
+void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
+{
+	ULOGGER_DEBUG("");
+	std::map<int, float> newPosterior;
+	for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
+	{
+		std::map<int, float>::iterator post = _posterior.find(*i);
+		if(post == _posterior.end())
+		{
+			if(_posterior.size() == 0)
+			{
+				newPosterior.insert(std::pair<int, float>(*i, 1));
+			}
+			else
+			{
+				newPosterior.insert(std::pair<int, float>(*i, 0));
+			}
+		}
+		else
+		{
+			newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
+		}
+	}
+	_posterior = newPosterior;
+}
+
+//recursive...
+float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int row, const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const
+{
+	if(!likelihoodIds.size() ||
+	   prediction == 0 ||
+	   prediction->rows != prediction->cols ||
+	   (unsigned int)prediction->rows != likelihoodIds.size() ||
+	   _predictionLC.size() < 2 ||
+	   !memory ||
+	   !prediction ||
+	   level<1)
+	{
+		ULOGGER_ERROR( "fail");
+		return 0;
+	}
+
+	if(level+1 >= _predictionLC.size() || !s)
+	{
+		return 0;
+	}
+
+	double value = _predictionLC[level+1];
+	float sum=0;
+
+	const NeighborsMap & neighbors = s->getNeighbors();
+	for(NeighborsMap::const_iterator iter=neighbors.begin(); iter!= neighbors.end(); ++iter)
+	{
+		int index = uValue(likelihoodIds, iter->first, -1);
+		if(index >= 0)
+		{
+			bool alreadyAdded = false;
+			// the value can be already added in the recursion
+			if(value > prediction->data.fl[row + index*prediction->cols])
+			{
+				sum -= prediction->data.fl[row + index*prediction->cols];
+				prediction->data.fl[row + index*prediction->cols] = value;
+				sum += value;
+			}
+			else
+			{
+				alreadyAdded = true;
+			}
+
+			if(!alreadyAdded && level+1 < _predictionLC.size())
+			{
+				sum += addNeighborProb(prediction, row, memory, likelihoodIds, memory->getSignature(iter->first), level+1);
+			}
+		}
+		else
+		{
+			//ULOGGER_DEBUG("BayesFilter::generatePrediction(...) F (id %d) Not found for loop %d", loopSign->getNeighborForward(), loopClosureId);
+		}
+	}
+	return sum;
+}
+
+
+} // namespace rtabmap

corelib/src/BayesFilter.h

@@ -0,0 +1,69 @@
+/*
+ * 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 BAYESFILTER_H_
+#define BAYESFILTER_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <list>
+#include <set>
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap {
+
+class Memory;
+class Signature;
+
+class RTABMAP_EXP BayesFilter
+{
+public:
+	BayesFilter(const ParametersMap & parameters = ParametersMap());
+	virtual ~BayesFilter();
+	virtual void parseParameters(const ParametersMap & parameters);
+	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+	void reset();
+
+	//setters
+	void setVirtualPlacePrior(float virtualPlacePrior);
+	void setPredictionLC(const std::string & prediction);
+
+	//getters
+	const std::map<int, float> & getPosterior() const {return _posterior;}
+	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
+	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+
+	bool generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const;
+	float addNeighborProb(CvMat * prediction, unsigned int row,  const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const;
+
+private:
+	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+
+private:
+	std::map<int, float> _posterior;
+	float _virtualPlacePrior;
+	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+};
+
+} // namespace rtabmap
+
+#endif /* BAYESFILTER_H_ */

corelib/src/CMakeLists.txt

@@ -0,0 +1,77 @@
+
+SET(SRC_FILES
+	Rtabmap.cpp
+	RtabmapEvent.cpp
+	
+	Memory.cpp
+	KeypointMemory.cpp
+	
+	DBDriverFactory.cpp
+	DBDriver.cpp
+	DBDriverSqlite3.cpp
+	
+    Camera.cpp
+    EpipolarGeometry.cpp
+	VisualWord.cpp
+	VWDictionary.cpp
+	BayesFilter.cpp
+	Parameters.cpp
+    Signature.cpp
+	KeypointDetector.cpp
+	KeypointDescriptor.cpp
+	VerifyHypotheses.cpp
+	NearestNeighbor.cpp
+)
+
+SET(INCLUDE_DIRS
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${CMAKE_CURRENT_BINARY_DIR}
+	${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+	${SQLITE3_INCLUDE_DIR}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${OpenCV_LIBS} 
+	${SQLITE3_LIBRARY}
+)
+
+# Generate resources files
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+   COMMENT "[Creating resources]"
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+IF(WIN32)
+    IF(BUILD_SHARED_LIBS)   
+	   ADD_DEFINITIONS(-DRTABMAP_EXPORTS) 
+	ELSE()
+	   ADD_DEFINITIONS(-DRTABMAP_EXPORTS_STATIC)
+	ENDIF()
+ENDIF(WIN32)
+
+# Add binary that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_LIBRARY(corelib ${SRC_FILES} ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h)
+TARGET_LINK_LIBRARIES(corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES(
+corelib 
+PROPERTIES
+ OUTPUT_NAME ${PROJECT_PREFIX}_core
+ INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
+)
+
+INSTALL(TARGETS corelib
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+		
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
+

corelib/src/Camera.cpp

@@ -0,0 +1,651 @@
+/*
+ * 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 "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraEvent.h"
+#include "utilite/UEventsManager.h"
+#include "utilite/UConversion.h"
+#include "rtabmap/core/DBDriver.h"
+#include "rtabmap/core/DBDriverFactory.h"
+#include "rtabmap/core/KeypointDescriptor.h"
+#include "rtabmap/core/KeypointDetector.h"
+#include "rtabmap/core/SMState.h"
+#include "utilite/UStl.h"
+#include "utilite/UConversion.h"
+#include "utilite/UFile.h"
+#include "utilite/UDirectory.h"
+#include "utilite/UTimer.h"
+#include <opencv2/imgproc/imgproc_c.h>
+
+namespace rtabmap
+{
+
+SMState * CamPostTreatment::process(IplImage * image)
+{
+	if(image)
+	{
+		return new SMState(image);
+	}
+	return 0;
+}
+
+CamKeypointTreatment::~CamKeypointTreatment()
+{
+	if(_keypointDetector)
+	{
+		delete _keypointDetector;
+	}
+	if(_keypointDescriptor)
+	{
+		delete _keypointDescriptor;
+	}
+}
+SMState * CamKeypointTreatment::process(IplImage * image)
+{
+	if(image)
+	{
+		std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(image);
+		std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
+		SMState * smState = new SMState(descriptors, std::list<std::vector<float> >(), image, keypoints);
+		return smState;
+	}
+	return 0;
+}
+
+void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
+{
+	UDEBUG("");
+	ParametersMap::const_iterator iter;
+	//Keypoint detector
+	DetectorStrategy detectorStrategy = kDetectorUndef;
+	if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
+	{
+		detectorStrategy = (DetectorStrategy)std::atoi((*iter).second.c_str());
+	}
+	DetectorStrategy currentDetectorStrategy = this->detectorStrategy();
+	if(!_keypointDetector || ( detectorStrategy!=kDetectorUndef && (detectorStrategy != currentDetectorStrategy) ) )
+	{
+		ULOGGER_DEBUG("new detector strategy %d", int(detectorStrategy));
+		if(_keypointDetector)
+		{
+			delete _keypointDetector;
+			_keypointDetector = 0;
+		}
+		switch(detectorStrategy)
+		{
+		case kDetectorStar:
+			_keypointDetector = new StarDetector(parameters);
+			break;
+		case kDetectorSift:
+			_keypointDetector = new SIFTDetector(parameters);
+			break;
+		case kDetectorSurf:
+		default:
+			_keypointDetector = new SURFDetector(parameters);
+			break;
+		}
+	}
+	else if(_keypointDetector)
+	{
+		_keypointDetector->parseParameters(parameters);
+	}
+
+	//Keypoint descriptor
+	DescriptorStrategy descriptorStrategy = kDescriptorUndef;
+	if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
+	{
+		descriptorStrategy = (DescriptorStrategy)std::atoi((*iter).second.c_str());
+	}
+	if(!_keypointDescriptor || descriptorStrategy!=kDescriptorUndef)
+	{
+		ULOGGER_DEBUG("new descriptor strategy %d", int(descriptorStrategy));
+		if(_keypointDescriptor)
+		{
+			delete _keypointDescriptor;
+			_keypointDescriptor = 0;
+		}
+		switch(descriptorStrategy)
+		{
+		case kDescriptorColorSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new ColorDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorLaplacianSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new LaplacianDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorSift:
+			_keypointDescriptor = new SIFTDescriptor(parameters);
+			break;
+		case kDescriptorHueSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new HueDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorSurf:
+		default:
+			_keypointDescriptor = new SURFDescriptor(parameters);
+			break;
+		}
+	}
+	else if(_keypointDescriptor)
+	{
+		_keypointDescriptor->parseParameters(parameters);
+	}
+	CamPostTreatment::parseParameters(parameters);
+}
+
+CamKeypointTreatment::DetectorStrategy CamKeypointTreatment::detectorStrategy() const
+{
+	DetectorStrategy strategy = kDetectorUndef;
+	StarDetector * star = dynamic_cast<StarDetector*>(_keypointDetector);
+	SURFDetector * surf = dynamic_cast<SURFDetector*>(_keypointDetector);
+	if(star)
+	{
+		strategy = kDetectorStar;
+	}
+	else if(surf)
+	{
+		strategy = kDetectorSurf;
+	}
+	return strategy;
+}
+
+Camera::Camera(float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight) :
+	_imageRate(imageRate),
+	_autoRestart(autoRestart),
+	_imageWidth(imageWidth),
+	_imageHeight(imageHeight)
+{
+	_postThreatement = new CamPostTreatment();
+	UEventsManager::addHandler(this);
+}
+
+Camera::~Camera(void)
+{
+	this->kill();
+	delete _postThreatement;
+}
+
+void Camera::mainLoop()
+{
+	State state = kStateCapturing;
+	ParametersMap parameters;
+
+	_stateMutex.lock();
+	{
+		if(!_state.empty() && !_stateParam.empty())
+		{
+			state = _state.top();
+			_state.pop();
+			parameters = _stateParam.top();
+			_stateParam.pop();
+		}
+	}
+	_stateMutex.unlock();
+
+	if(state == kStateCapturing)
+	{
+		process();
+	}
+	else if(state == kStateChangingParameters)
+	{
+		this->parseParameters(parameters);
+	}
+}
+
+// ownership is transferred
+void Camera::setPostThreatement(CamPostTreatment * strategy)
+{
+	if(strategy)
+	{
+		delete _postThreatement;
+		_postThreatement = strategy;
+	}
+}
+
+void Camera::pushNewState(State newState, const ParametersMap & parameters)
+{
+	ULOGGER_DEBUG("to %d", newState);
+
+	_stateMutex.lock();
+	{
+		_state.push(newState);
+		_stateParam.push(parameters);
+	}
+	_stateMutex.unlock();
+}
+
+void Camera::handleEvent(UEvent* anEvent)
+{
+	if(anEvent->getClassName().compare("CameraEvent") == 0)
+	{
+		CameraEvent * cameraEvent = (CameraEvent*)anEvent;
+		if(cameraEvent->getCode() == CameraEvent::kCodeCtrl)
+		{
+			CameraEvent::Cmd cmd = cameraEvent->getCommand();
+
+			if(cmd == CameraEvent::kCmdPause)
+			{
+				if(this->isRunning())
+				{
+					this->kill();
+				}
+				else
+				{
+					this->start();
+				}
+			}
+			else if(cmd == CameraEvent::kCmdChangeParam)
+			{
+				// TODO : Put in global Parameters ?
+				_imageRate = cameraEvent->getImageRate();
+				_autoRestart = cameraEvent->getAutoRestart();
+			}
+			else
+			{
+				ULOGGER_DEBUG("Camera::handleEvent(Util::Event* anEvent) : command undefined...");
+			}
+		}
+	}
+	if(anEvent->getClassName().compare("ParamEvent") == 0)
+	{
+		if(this->isIdle())
+		{
+			_stateMutex.lock();
+			this->parseParameters(((ParamEvent*)anEvent)->getParameters());
+			_stateMutex.unlock();
+		}
+		else
+		{
+			ULOGGER_DEBUG("changing parameters");
+			pushNewState(kStateChangingParameters, ((ParamEvent*)anEvent)->getParameters());
+		}
+	}
+}
+
+void Camera::process()
+{
+	UTimer timer;
+	ULOGGER_DEBUG("Camera::process()");
+	IplImage * image = this->takeImage();
+	if(image)
+	{
+		SMState * smState = _postThreatement->process(image);
+		this->post(new SMStateEvent(smState));
+		double elapsed = timer.ticks();
+		UDEBUG("Post treatment time = %fs", elapsed);
+		if(_imageRate>0)
+		{
+			float sleepTime = 1000.0f/_imageRate - 1000.0f*elapsed;
+			if(sleepTime > 0)
+			{
+				UDEBUG("Now sleeping for = %fms", sleepTime);
+				uSleep(sleepTime);
+			}
+		}
+	}
+	else
+	{
+		if(_autoRestart)
+		{
+			this->init();
+		}
+		else
+		{
+			ULOGGER_DEBUG("Camera::process() : no more images...");
+			this->kill();
+			this->post(new CameraEvent());
+		}
+	}
+}
+
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+CameraImages::CameraImages(const std::string & path,
+					 int startAt,
+					 bool refreshDir,
+					 float imageRate,
+					 bool autoRestart,
+					 unsigned int imageWidth,
+					 unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_path(path),
+	_startAt(startAt),
+	_refreshDir(refreshDir),
+	_dir(0),
+	_count(0)
+{
+}
+
+CameraImages::~CameraImages(void)
+{
+	this->kill();
+	if(_dir)
+	{
+		delete _dir;
+		_dir = 0;
+	}
+}
+
+bool CameraImages::init()
+{
+	if(_dir)
+	{
+		delete _dir;
+		_dir = 0;
+	}
+	_dir = new UDirectory(_path, "jpg ppm png bmp");
+	_count = 0;
+	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
+	{
+		_path.append("/");
+	}
+	if(!_dir)
+	{
+		ULOGGER_ERROR("Directory path not valid \"%s\"", _path.c_str());
+	}
+	return _dir != 0;
+}
+
+IplImage * CameraImages::takeImage()
+{
+	IplImage * img = 0;
+	if(_dir)
+	{
+		if(_refreshDir)
+		{
+			_dir->update();
+		}
+		if(_startAt == 0)
+		{
+			const std::list<std::string> & fileNames = _dir->getFileNames();
+			if(fileNames.size())
+			{
+				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
+				{
+					_lastFileName = *fileNames.rbegin();
+					std::string fullPath = _path + _lastFileName;
+					img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_UNCHANGED);
+				}
+			}
+		}
+		else
+		{
+			std::string fileName;
+			std::string fullPath;
+			fileName = _dir->getNextFileName();
+			if(fileName.size())
+			{
+				fullPath = _path + fileName;
+				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
+				{
+					fullPath = _path + fileName;
+				}
+				if(fileName.size())
+				{
+					ULOGGER_DEBUG("Loading image : %s\n", fullPath.c_str());
+					img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_UNCHANGED);
+				}
+			}
+		}
+	}
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		cvReleaseImage(&img);
+		img = resampledImg;
+	}
+	return img;
+}
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+CameraVideo::CameraVideo(int usbDevice,
+						 float imageRate,
+						 bool autoRestart,
+						 unsigned int imageWidth,
+						  unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_capture(0),
+	_src(kUsbDevice),
+	_usbDevice(usbDevice)
+{
+
+}
+
+CameraVideo::CameraVideo(const std::string & fileName,
+						   float imageRate,
+						   bool autoRestart,
+						   unsigned int imageWidth,
+						   unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_fileName(fileName),
+	_capture(0),
+	_src(kVideoFile)
+{
+}
+
+CameraVideo::~CameraVideo()
+{
+	this->kill();
+	if(_capture)
+	{
+		cvReleaseCapture(&_capture);
+	}
+}
+
+bool CameraVideo::init()
+{
+	if(_capture)
+	{
+		cvReleaseCapture(&_capture);
+		_capture = 0;
+	}
+
+	if(_src == kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, getImageWidth(), getImageHeight());
+		_capture = cvCaptureFromCAM(_usbDevice);
+		if(_capture && getImageWidth() && getImageHeight())
+		{
+			cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_WIDTH, double(getImageWidth()));
+			cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, double(getImageHeight()));
+		}
+	}
+	else if(_src == kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() filename=\"%s\"", _fileName.c_str());
+		_capture = cvCaptureFromAVI(_fileName.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraVideo::init() Unknown source...");
+	}
+	if(!_capture)
+	{
+		ULOGGER_ERROR("CameraVideo::init() Failed to create a capture object!");
+		return false;
+	}
+	return true;
+}
+
+IplImage * CameraVideo::takeImage()
+{
+	IplImage * img = 0;  // Null image
+	if(_capture)
+	{
+		if(!cvGrabFrame(_capture)){              // capture a frame
+			ULOGGER_WARN("CameraVideo: Could not grab a frame, the end of the feed may be reached...");
+		}
+		else
+		{
+			img=cvRetrieveFrame(_capture);           // retrieve the captured frame
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("CameraVideo::takeImage() The camera must be initialized before requesting an image.");
+	}
+
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		img = resampledImg;
+	}
+	else if(img)
+	{
+		img = cvCloneImage(img);
+	}
+
+	return img;
+}
+
+
+
+
+
+
+
+/////////////////////////
+// CameraDatabase
+/////////////////////////
+CameraDatabase::CameraDatabase(const std::string & path,
+								bool ignoreChildren,
+								float imageRate,
+								bool autoRestart,
+								unsigned int imageWidth,
+								unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_path(path),
+	_ignoreChildren(ignoreChildren),
+	_indexIter(_ids.begin()),
+	_dbDriver(0)
+{
+}
+
+CameraDatabase::~CameraDatabase(void)
+{
+	this->kill();
+	if(_dbDriver)
+	{
+		_dbDriver->closeConnection();
+		delete _dbDriver;
+	}
+}
+
+bool CameraDatabase::init()
+{
+	if(_dbDriver)
+	{
+		_dbDriver->closeConnection();
+		delete _dbDriver;
+		_dbDriver = 0;
+	}
+	_ids.clear();
+	_indexIter = _ids.begin();
+
+	std::string driverType = "sqlite3";
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+	_dbDriver = rtabmap::DBDriverFactory::createDBDriver(driverType, parameters);
+	if(!_dbDriver)
+	{
+		ULOGGER_ERROR("CameraDatabase::init() can't create \"%s\" driver",driverType.c_str());
+		return false;
+	}
+	else if(!_dbDriver->openConnection(_path.c_str()))
+	{
+		ULOGGER_ERROR("CameraDatabase::init() Can't read database \"%s\"",_path.c_str());
+		return false;
+	}
+	else
+	{
+		// TODO load all signatures only if ignoreChildren is false
+		_dbDriver->getAllSignatureIds(_ids);
+		_indexIter = _ids.begin();
+	}
+	return true;
+}
+
+IplImage * CameraDatabase::takeImage()
+{
+	IplImage * img = 0;
+	if(_dbDriver && _indexIter != _ids.end())
+	{
+		_dbDriver->getImage(*_indexIter, &img);
+		++_indexIter;
+	}
+	else if(!_dbDriver)
+	{
+		ULOGGER_WARN("The camera must be initialized first...");
+	}
+	else if(_ids.size() == 0)
+	{
+		ULOGGER_WARN("The database \"%s\" is empty...", _path.c_str());
+	}
+
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		cvReleaseImage(&img);
+		img = resampledImg;
+	}
+
+	return img;
+}
+
+} // namespace rtabmap

corelib/src/ConvertUTF.c

@@ -0,0 +1,539 @@
+/*
+ * Copyright 2001-2004 Unicode, Inc.
+ * 
+ * Disclaimer
+ * 
+ * This source code is provided as is by Unicode, Inc. No claims are
+ * made as to fitness for any particular purpose. No warranties of any
+ * kind are expressed or implied. The recipient agrees to determine
+ * applicability of information provided. If this file has been
+ * purchased on magnetic or optical media from Unicode, Inc., the
+ * sole remedy for any claim will be exchange of defective media
+ * within 90 days of receipt.
+ * 
+ * Limitations on Rights to Redistribute This Code
+ * 
+ * Unicode, Inc. hereby grants the right to freely use the information
+ * supplied in this file in the creation of products supporting the
+ * Unicode Standard, and to make copies of this file in any form
+ * for internal or external distribution as long as this notice
+ * remains attached.
+ */
+
+/* ---------------------------------------------------------------------
+
+    Conversions between UTF32, UTF-16, and UTF-8. Source code file.
+    Author: Mark E. Davis, 1994.
+    Rev History: Rick McGowan, fixes & updates May 2001.
+    Sept 2001: fixed const & error conditions per
+	mods suggested by S. Parent & A. Lillich.
+    June 2002: Tim Dodd added detection and handling of incomplete
+	source sequences, enhanced error detection, added casts
+	to eliminate compiler warnings.
+    July 2003: slight mods to back out aggressive FFFE detection.
+    Jan 2004: updated switches in from-UTF8 conversions.
+    Oct 2004: updated to use UNI_MAX_LEGAL_UTF32 in UTF-32 conversions.
+
+    See the header file "ConvertUTF.h" for complete documentation.
+
+------------------------------------------------------------------------ */
+
+
+#include "ConvertUTF.h"
+#ifdef CVTUTF_DEBUG
+#include <stdio.h>
+#endif
+
+static const int halfShift  = 10; /* used for shifting by 10 bits */
+
+static const UTF32 halfBase = 0x0010000UL;
+static const UTF32 halfMask = 0x3FFUL;
+
+#define UNI_SUR_HIGH_START  (UTF32)0xD800
+#define UNI_SUR_HIGH_END    (UTF32)0xDBFF
+#define UNI_SUR_LOW_START   (UTF32)0xDC00
+#define UNI_SUR_LOW_END     (UTF32)0xDFFF
+#define false	   0
+#define true	    1
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF32toUTF16 (
+	const UTF32** sourceStart, const UTF32* sourceEnd, 
+	UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF32* source = *sourceStart;
+    UTF16* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	if (target >= targetEnd) {
+	    result = targetExhausted; break;
+	}
+	ch = *source++;
+	if (ch <= UNI_MAX_BMP) { /* Target is a character <= 0xFFFF */
+	    /* UTF-16 surrogate values are illegal in UTF-32; 0xffff or 0xfffe are both reserved values */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = (UTF16)ch; /* normal case */
+	    }
+	} else if (ch > UNI_MAX_LEGAL_UTF32) {
+	    if (flags == strictConversion) {
+		result = sourceIllegal;
+	    } else {
+		*target++ = UNI_REPLACEMENT_CHAR;
+	    }
+	} else {
+	    /* target is a character in range 0xFFFF - 0x10FFFF. */
+	    if (target + 1 >= targetEnd) {
+		--source; /* Back up source pointer! */
+		result = targetExhausted; break;
+	    }
+	    ch -= halfBase;
+	    *target++ = (UTF16)((ch >> halfShift) + UNI_SUR_HIGH_START);
+	    *target++ = (UTF16)((ch & halfMask) + UNI_SUR_LOW_START);
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF16toUTF32 (
+	const UTF16** sourceStart, const UTF16* sourceEnd, 
+	UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF16* source = *sourceStart;
+    UTF32* target = *targetStart;
+    UTF32 ch, ch2;
+    while (source < sourceEnd) {
+	const UTF16* oldSource = source; /*  In case we have to back up because of target overflow. */
+	ch = *source++;
+	/* If we have a surrogate pair, convert to UTF32 first. */
+	if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_HIGH_END) {
+	    /* If the 16 bits following the high surrogate are in the source buffer... */
+	    if (source < sourceEnd) {
+		ch2 = *source;
+		/* If it's a low surrogate, convert to UTF32. */
+		if (ch2 >= UNI_SUR_LOW_START && ch2 <= UNI_SUR_LOW_END) {
+		    ch = ((ch - UNI_SUR_HIGH_START) << halfShift)
+			+ (ch2 - UNI_SUR_LOW_START) + halfBase;
+		    ++source;
+		} else if (flags == strictConversion) { /* it's an unpaired high surrogate */
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		}
+	    } else { /* We don't have the 16 bits following the high surrogate. */
+		--source; /* return to the high surrogate */
+		result = sourceExhausted;
+		break;
+	    }
+	} else if (flags == strictConversion) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_LOW_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	if (target >= targetEnd) {
+	    source = oldSource; /* Back up source pointer! */
+	    result = targetExhausted; break;
+	}
+	*target++ = ch;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+#ifdef CVTUTF_DEBUG
+if (result == sourceIllegal) {
+    fprintf(stderr, "ConvertUTF16toUTF32 illegal seq 0x%04x,%04x\n", ch, ch2);
+    fflush(stderr);
+}
+#endif
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Index into the table below with the first byte of a UTF-8 sequence to
+ * get the number of trailing bytes that are supposed to follow it.
+ * Note that *legal* UTF-8 values can't have 4 or 5-bytes. The table is
+ * left as-is for anyone who may want to do such conversion, which was
+ * allowed in earlier algorithms.
+ */
+static const char trailingBytesForUTF8[256] = {
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
+    2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 3,3,3,3,3,3,3,3,4,4,4,4,5,5,5,5
+};
+
+/*
+ * Magic values subtracted from a buffer value during UTF8 conversion.
+ * This table contains as many values as there might be trailing bytes
+ * in a UTF-8 sequence.
+ */
+static const UTF32 offsetsFromUTF8[6] = { 0x00000000UL, 0x00003080UL, 0x000E2080UL, 
+		     0x03C82080UL, 0xFA082080UL, 0x82082080UL };
+
+/*
+ * Once the bits are split out into bytes of UTF-8, this is a mask OR-ed
+ * into the first byte, depending on how many bytes follow.  There are
+ * as many entries in this table as there are UTF-8 sequence types.
+ * (I.e., one byte sequence, two byte... etc.). Remember that sequencs
+ * for *legal* UTF-8 will be 4 or fewer bytes total.
+ */
+static const UTF8 firstByteMark[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC };
+
+/* --------------------------------------------------------------------- */
+
+/* The interface converts a whole buffer to avoid function-call overhead.
+ * Constants have been gathered. Loops & conditionals have been removed as
+ * much as possible for efficiency, in favor of drop-through switches.
+ * (See "Note A" at the bottom of the file for equivalent code.)
+ * If your compiler supports it, the "isLegalUTF8" call can be turned
+ * into an inline function.
+ */
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF16toUTF8 (
+	const UTF16** sourceStart, const UTF16* sourceEnd, 
+	UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF16* source = *sourceStart;
+    UTF8* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	unsigned short bytesToWrite = 0;
+	const UTF32 byteMask = 0xBF;
+	const UTF32 byteMark = 0x80; 
+	const UTF16* oldSource = source; /* In case we have to back up because of target overflow. */
+	ch = *source++;
+	/* If we have a surrogate pair, convert to UTF32 first. */
+	if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_HIGH_END) {
+	    /* If the 16 bits following the high surrogate are in the source buffer... */
+	    if (source < sourceEnd) {
+		UTF32 ch2 = *source;
+		/* If it's a low surrogate, convert to UTF32. */
+		if (ch2 >= UNI_SUR_LOW_START && ch2 <= UNI_SUR_LOW_END) {
+		    ch = ((ch - UNI_SUR_HIGH_START) << halfShift)
+			+ (ch2 - UNI_SUR_LOW_START) + halfBase;
+		    ++source;
+		} else if (flags == strictConversion) { /* it's an unpaired high surrogate */
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		}
+	    } else { /* We don't have the 16 bits following the high surrogate. */
+		--source; /* return to the high surrogate */
+		result = sourceExhausted;
+		break;
+	    }
+	} else if (flags == strictConversion) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_LOW_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	/* Figure out how many bytes the result will require */
+	if (ch < (UTF32)0x80) {	     bytesToWrite = 1;
+	} else if (ch < (UTF32)0x800) {     bytesToWrite = 2;
+	} else if (ch < (UTF32)0x10000) {   bytesToWrite = 3;
+	} else if (ch < (UTF32)0x110000) {  bytesToWrite = 4;
+	} else {			    bytesToWrite = 3;
+					    ch = UNI_REPLACEMENT_CHAR;
+	}
+
+	target += bytesToWrite;
+	if (target > targetEnd) {
+	    source = oldSource; /* Back up source pointer! */
+	    target -= bytesToWrite; result = targetExhausted; break;
+	}
+	switch (bytesToWrite) { /* note: everything falls through. */
+	    case 4: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 3: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 2: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 1: *--target =  (UTF8)(ch | firstByteMark[bytesToWrite]);
+	}
+	target += bytesToWrite;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Utility routine to tell whether a sequence of bytes is legal UTF-8.
+ * This must be called with the length pre-determined by the first byte.
+ * If not calling this from ConvertUTF8to*, then the length can be set by:
+ *  length = trailingBytesForUTF8[*source]+1;
+ * and the sequence is illegal right away if there aren't that many bytes
+ * available.
+ * If presented with a length > 4, this returns false.  The Unicode
+ * definition of UTF-8 goes up to 4-byte sequences.
+ */
+
+static Boolean isLegalUTF8(const UTF8 *source, int length) {
+    UTF8 a;
+    const UTF8 *srcptr = source+length;
+    switch (length) {
+    default: return false;
+	/* Everything else falls through when "true"... */
+    case 4: if ((a = (*--srcptr)) < 0x80 || a > 0xBF) return false;
+    case 3: if ((a = (*--srcptr)) < 0x80 || a > 0xBF) return false;
+    case 2: if ((a = (*--srcptr)) > 0xBF) return false;
+
+	switch (*source) {
+	    /* no fall-through in this inner switch */
+	    case 0xE0: if (a < 0xA0) return false; break;
+	    case 0xED: if (a > 0x9F) return false; break;
+	    case 0xF0: if (a < 0x90) return false; break;
+	    case 0xF4: if (a > 0x8F) return false; break;
+	    default:   if (a < 0x80) return false;
+	}
+
+    case 1: if (*source >= 0x80 && *source < 0xC2) return false;
+    }
+    if (*source > 0xF4) return false;
+    return true;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Exported function to return whether a UTF-8 sequence is legal or not.
+ * This is not used here; it's just exported.
+ */
+Boolean isLegalUTF8Sequence(const UTF8 *source, const UTF8 *sourceEnd) {
+    int length = trailingBytesForUTF8[*source]+1;
+    if (source+length > sourceEnd) {
+	return false;
+    }
+    return isLegalUTF8(source, length);
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF8toUTF16 (
+	const UTF8** sourceStart, const UTF8* sourceEnd, 
+	UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF8* source = *sourceStart;
+    UTF16* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch = 0;
+	unsigned short extraBytesToRead = trailingBytesForUTF8[*source];
+	if (source + extraBytesToRead >= sourceEnd) {
+	    result = sourceExhausted; break;
+	}
+	/* Do this check whether lenient or strict */
+	if (! isLegalUTF8(source, extraBytesToRead+1)) {
+	    result = sourceIllegal;
+	    break;
+	}
+	/*
+	 * The cases all fall through. See "Note A" below.
+	 */
+	switch (extraBytesToRead) {
+	    case 5: ch += *source++; ch <<= 6; /* remember, illegal UTF-8 */
+	    case 4: ch += *source++; ch <<= 6; /* remember, illegal UTF-8 */
+	    case 3: ch += *source++; ch <<= 6;
+	    case 2: ch += *source++; ch <<= 6;
+	    case 1: ch += *source++; ch <<= 6;
+	    case 0: ch += *source++;
+	}
+	ch -= offsetsFromUTF8[extraBytesToRead];
+
+	if (target >= targetEnd) {
+	    source -= (extraBytesToRead+1); /* Back up source pointer! */
+	    result = targetExhausted; break;
+	}
+	if (ch <= UNI_MAX_BMP) { /* Target is a character <= 0xFFFF */
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    source -= (extraBytesToRead+1); /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = (UTF16)ch; /* normal case */
+	    }
+	} else if (ch > UNI_MAX_UTF16) {
+	    if (flags == strictConversion) {
+		result = sourceIllegal;
+		source -= (extraBytesToRead+1); /* return to the start */
+		break; /* Bail out; shouldn't continue */
+	    } else {
+		*target++ = UNI_REPLACEMENT_CHAR;
+	    }
+	} else {
+	    /* target is a character in range 0xFFFF - 0x10FFFF. */
+	    if (target + 1 >= targetEnd) {
+		source -= (extraBytesToRead+1); /* Back up source pointer! */
+		result = targetExhausted; break;
+	    }
+	    ch -= halfBase;
+	    *target++ = (UTF16)((ch >> halfShift) + UNI_SUR_HIGH_START);
+	    *target++ = (UTF16)((ch & halfMask) + UNI_SUR_LOW_START);
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF32toUTF8 (
+	const UTF32** sourceStart, const UTF32* sourceEnd, 
+	UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF32* source = *sourceStart;
+    UTF8* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	unsigned short bytesToWrite = 0;
+	const UTF32 byteMask = 0xBF;
+	const UTF32 byteMark = 0x80; 
+	ch = *source++;
+	if (flags == strictConversion ) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	/*
+	 * Figure out how many bytes the result will require. Turn any
+	 * illegally large UTF32 things (> Plane 17) into replacement chars.
+	 */
+	if (ch < (UTF32)0x80) {	     bytesToWrite = 1;
+	} else if (ch < (UTF32)0x800) {     bytesToWrite = 2;
+	} else if (ch < (UTF32)0x10000) {   bytesToWrite = 3;
+	} else if (ch <= UNI_MAX_LEGAL_UTF32) {  bytesToWrite = 4;
+	} else {			    bytesToWrite = 3;
+					    ch = UNI_REPLACEMENT_CHAR;
+					    result = sourceIllegal;
+	}
+	
+	target += bytesToWrite;
+	if (target > targetEnd) {
+	    --source; /* Back up source pointer! */
+	    target -= bytesToWrite; result = targetExhausted; break;
+	}
+	switch (bytesToWrite) { /* note: everything falls through. */
+	    case 4: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 3: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 2: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 1: *--target = (UTF8) (ch | firstByteMark[bytesToWrite]);
+	}
+	target += bytesToWrite;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF8toUTF32 (
+	const UTF8** sourceStart, const UTF8* sourceEnd, 
+	UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF8* source = *sourceStart;
+    UTF32* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch = 0;
+	unsigned short extraBytesToRead = trailingBytesForUTF8[*source];
+	if (source + extraBytesToRead >= sourceEnd) {
+	    result = sourceExhausted; break;
+	}
+	/* Do this check whether lenient or strict */
+	if (! isLegalUTF8(source, extraBytesToRead+1)) {
+	    result = sourceIllegal;
+	    break;
+	}
+	/*
+	 * The cases all fall through. See "Note A" below.
+	 */
+	switch (extraBytesToRead) {
+	    case 5: ch += *source++; ch <<= 6;
+	    case 4: ch += *source++; ch <<= 6;
+	    case 3: ch += *source++; ch <<= 6;
+	    case 2: ch += *source++; ch <<= 6;
+	    case 1: ch += *source++; ch <<= 6;
+	    case 0: ch += *source++;
+	}
+	ch -= offsetsFromUTF8[extraBytesToRead];
+
+	if (target >= targetEnd) {
+	    source -= (extraBytesToRead+1); /* Back up the source pointer! */
+	    result = targetExhausted; break;
+	}
+	if (ch <= UNI_MAX_LEGAL_UTF32) {
+	    /*
+	     * UTF-16 surrogate values are illegal in UTF-32, and anything
+	     * over Plane 17 (> 0x10FFFF) is illegal.
+	     */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    source -= (extraBytesToRead+1); /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = ch;
+	    }
+	} else { /* i.e., ch > UNI_MAX_LEGAL_UTF32 */
+	    result = sourceIllegal;
+	    *target++ = UNI_REPLACEMENT_CHAR;
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* ---------------------------------------------------------------------
+
+    Note A.
+    The fall-through switches in UTF-8 reading code save a
+    temp variable, some decrements & conditionals.  The switches
+    are equivalent to the following loop:
+	{
+	    int tmpBytesToRead = extraBytesToRead+1;
+	    do {
+		ch += *source++;
+		--tmpBytesToRead;
+		if (tmpBytesToRead) ch <<= 6;
+	    } while (tmpBytesToRead > 0);
+	}
+    In UTF-8 writing code, the switches on "bytesToWrite" are
+    similarly unrolled loops.
+
+   --------------------------------------------------------------------- */

corelib/src/ConvertUTF.h

@@ -0,0 +1,149 @@
+/*
+ * Copyright 2001-2004 Unicode, Inc.
+ * 
+ * Disclaimer
+ * 
+ * This source code is provided as is by Unicode, Inc. No claims are
+ * made as to fitness for any particular purpose. No warranties of any
+ * kind are expressed or implied. The recipient agrees to determine
+ * applicability of information provided. If this file has been
+ * purchased on magnetic or optical media from Unicode, Inc., the
+ * sole remedy for any claim will be exchange of defective media
+ * within 90 days of receipt.
+ * 
+ * Limitations on Rights to Redistribute This Code
+ * 
+ * Unicode, Inc. hereby grants the right to freely use the information
+ * supplied in this file in the creation of products supporting the
+ * Unicode Standard, and to make copies of this file in any form
+ * for internal or external distribution as long as this notice
+ * remains attached.
+ */
+
+/* ---------------------------------------------------------------------
+
+    Conversions between UTF32, UTF-16, and UTF-8.  Header file.
+
+    Several funtions are included here, forming a complete set of
+    conversions between the three formats.  UTF-7 is not included
+    here, but is handled in a separate source file.
+
+    Each of these routines takes pointers to input buffers and output
+    buffers.  The input buffers are const.
+
+    Each routine converts the text between *sourceStart and sourceEnd,
+    putting the result into the buffer between *targetStart and
+    targetEnd. Note: the end pointers are *after* the last item: e.g. 
+    *(sourceEnd - 1) is the last item.
+
+    The return result indicates whether the conversion was successful,
+    and if not, whether the problem was in the source or target buffers.
+    (Only the first encountered problem is indicated.)
+
+    After the conversion, *sourceStart and *targetStart are both
+    updated to point to the end of last text successfully converted in
+    the respective buffers.
+
+    Input parameters:
+	sourceStart - pointer to a pointer to the source buffer.
+		The contents of this are modified on return so that
+		it points at the next thing to be converted.
+	targetStart - similarly, pointer to pointer to the target buffer.
+	sourceEnd, targetEnd - respectively pointers to the ends of the
+		two buffers, for overflow checking only.
+
+    These conversion functions take a ConversionFlags argument. When this
+    flag is set to strict, both irregular sequences and isolated surrogates
+    will cause an error.  When the flag is set to lenient, both irregular
+    sequences and isolated surrogates are converted.
+
+    Whether the flag is strict or lenient, all illegal sequences will cause
+    an error return. This includes sequences such as: <F4 90 80 80>, <C0 80>,
+    or <A0> in UTF-8, and values above 0x10FFFF in UTF-32. Conformant code
+    must check for illegal sequences.
+
+    When the flag is set to lenient, characters over 0x10FFFF are converted
+    to the replacement character; otherwise (when the flag is set to strict)
+    they constitute an error.
+
+    Output parameters:
+	The value "sourceIllegal" is returned from some routines if the input
+	sequence is malformed.  When "sourceIllegal" is returned, the source
+	value will point to the illegal value that caused the problem. E.g.,
+	in UTF-8 when a sequence is malformed, it points to the start of the
+	malformed sequence.  
+
+    Author: Mark E. Davis, 1994.
+    Rev History: Rick McGowan, fixes & updates May 2001.
+		 Fixes & updates, Sept 2001.
+
+------------------------------------------------------------------------ */
+
+/* ---------------------------------------------------------------------
+    The following 4 definitions are compiler-specific.
+    The C standard does not guarantee that wchar_t has at least
+    16 bits, so wchar_t is no less portable than unsigned short!
+    All should be unsigned values to avoid sign extension during
+    bit mask & shift operations.
+------------------------------------------------------------------------ */
+
+typedef unsigned int	UTF32;	/* at least 32 bits */
+typedef unsigned short	UTF16;	/* at least 16 bits */
+typedef unsigned char	UTF8;	/* typically 8 bits */
+typedef unsigned char	Boolean; /* 0 or 1 */
+
+/* Some fundamental constants */
+#define UNI_REPLACEMENT_CHAR (UTF32)0x0000FFFD
+#define UNI_MAX_BMP (UTF32)0x0000FFFF
+#define UNI_MAX_UTF16 (UTF32)0x0010FFFF
+#define UNI_MAX_UTF32 (UTF32)0x7FFFFFFF
+#define UNI_MAX_LEGAL_UTF32 (UTF32)0x0010FFFF
+
+typedef enum {
+	conversionOK, 		/* conversion successful */
+	sourceExhausted,	/* partial character in source, but hit end */
+	targetExhausted,	/* insuff. room in target for conversion */
+	sourceIllegal		/* source sequence is illegal/malformed */
+} ConversionResult;
+
+typedef enum {
+	strictConversion = 0,
+	lenientConversion
+} ConversionFlags;
+
+/* This is for C++ and does no harm in C */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ConversionResult ConvertUTF8toUTF16 (
+		const UTF8** sourceStart, const UTF8* sourceEnd, 
+		UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF16toUTF8 (
+		const UTF16** sourceStart, const UTF16* sourceEnd, 
+		UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags);
+		
+ConversionResult ConvertUTF8toUTF32 (
+		const UTF8** sourceStart, const UTF8* sourceEnd, 
+		UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF32toUTF8 (
+		const UTF32** sourceStart, const UTF32* sourceEnd, 
+		UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags);
+		
+ConversionResult ConvertUTF16toUTF32 (
+		const UTF16** sourceStart, const UTF16* sourceEnd, 
+		UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF32toUTF16 (
+		const UTF32** sourceStart, const UTF32* sourceEnd, 
+		UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags);
+
+Boolean isLegalUTF8Sequence(const UTF8 *source, const UTF8 *sourceEnd);
+
+#ifdef __cplusplus
+}
+#endif
+
+/* --------------------------------------------------------------------- */

corelib/src/DBDriver.cpp

@@ -0,0 +1,774 @@
+/*
+ * 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 "rtabmap/core/DBDriver.h"
+
+#include "Signature.h"
+#include "VWDictionary.h"
+#include "utilite/UConversion.h"
+#include "utilite/UMath.h"
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UStl.h"
+
+namespace rtabmap {
+
+DBDriver::DBDriver(const ParametersMap & parameters) :
+	_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
+	_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
+	_asyncWaiting(true),
+	_emptyTrashesTime(0)
+{
+	this->parseParameters(parameters);
+}
+
+DBDriver::~DBDriver()
+{
+	this->kill();
+	this->emptyTrashes();
+}
+
+void DBDriver::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kDbMinSignaturesToSave())) != parameters.end())
+	{
+		_minSignaturesToSave = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kDbMinWordsToSave())) != parameters.end())
+	{
+		_minWordsToSave = std::atoi((*iter).second.c_str());
+	}
+}
+
+void DBDriver::closeConnection()
+{
+	this->kill();
+	this->emptyTrashes();
+	_dbSafeAccessMutex.lock();
+	this->disconnectDatabaseQuery();
+	_dbSafeAccessMutex.unlock();
+}
+
+bool DBDriver::openConnection(const std::string & url)
+{
+	_url = url;
+	_dbSafeAccessMutex.lock();
+	if(this->connectDatabaseQuery(url))
+	{
+		this->start();
+		_dbSafeAccessMutex.unlock();
+		return true;
+	}
+	_dbSafeAccessMutex.unlock();
+	return false;
+}
+
+bool DBDriver::isConnected() const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = isConnectedQuery();
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+// In bytes
+long DBDriver::getMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
+
+void DBDriver::mainLoop()
+{
+	UDEBUG("");
+	this->emptyTrashes();
+	this->kill(); // Do it only once
+	UDEBUG("");
+}
+
+void DBDriver::killCleanup()
+{
+	UDEBUG("");
+}
+
+void DBDriver::beginTransaction() const
+{
+	_transactionMutex.lock();
+	ULOGGER_DEBUG("");
+	this->executeNoResultQuery("BEGIN TRANSACTION;");
+}
+
+void DBDriver::commit() const
+{
+	ULOGGER_DEBUG("");
+	this->executeNoResultQuery("COMMIT;");
+	_transactionMutex.unlock();
+}
+
+bool DBDriver::executeNoResult(const std::string & sql) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->executeNoResultQuery(sql);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+void DBDriver::emptyTrashes(bool async)
+{
+	ULOGGER_DEBUG("");
+	if(async)
+	{
+		ULOGGER_DEBUG("Async emptying, start the trash thread");
+		this->start();
+		return;
+	}
+
+	UTimer totalTime;
+	totalTime.start();
+
+	std::vector<Signature*> signatures;
+	std::map<int, VisualWord*> visualWords;
+	_trashesMutex.lock();
+	{
+		signatures = uValues(_trashSignatures);
+		visualWords = _trashVisualWords;
+		_trashSignatures.clear();
+		_trashVisualWords.clear();
+
+		_asyncWaiting = true;
+
+		_dbSafeAccessMutex.lock();
+	}
+	_trashesMutex.unlock();
+
+	if(signatures.size() || visualWords.size())
+	{
+		ULOGGER_DEBUG("trashSignatures size = %d, trashVisualWords size = %d", signatures.size(), visualWords.size());
+		this->beginTransaction();
+		UTimer timer;
+		timer.start();
+		if(signatures.size())
+		{
+			if(this->isConnected())
+			{
+				//Only one query to the database
+				this->saveOrUpdate(signatures);
+			}
+
+			for(std::vector<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+			{
+				delete *iter;
+			}
+			signatures.clear();
+		}
+		ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
+		if(visualWords.size())
+		{
+			if(this->isConnected())
+			{
+				//Only one query to the database
+				this->saveQuery(uValues(visualWords));
+			}
+
+			for(std::map<int, VisualWord *>::iterator iter=visualWords.begin(); iter!=visualWords.end(); ++iter)
+			{
+				delete (*iter).second;
+			}
+			visualWords.clear();
+		}
+		ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
+		this->commit();
+	}
+
+	_emptyTrashesTime = totalTime.ticks();
+	ULOGGER_DEBUG("Total time emptying trashes = %fs...", _emptyTrashesTime);
+
+	_dbSafeAccessMutex.unlock();
+}
+
+void DBDriver::asyncSave(Signature * s)
+{
+	_trashesMutex.lock();
+	{
+		_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
+		if(_trashSignatures.size() > _minSignaturesToSave && this->isRunning() && _asyncWaiting)
+		{
+			ULOGGER_DEBUG("(Sign) Releasing addSem...");
+			_asyncWaiting = false;
+			this->start();
+		}
+	}
+	_trashesMutex.unlock();
+}
+
+void DBDriver::asyncSave(VisualWord * vw)
+{
+	_trashesMutex.lock();
+	{
+		_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
+		if(_trashVisualWords.size() > _minWordsToSave && this->isRunning() && _asyncWaiting)
+		{
+			ULOGGER_DEBUG("(Word) Releasing addSem...");
+			_asyncWaiting = false;
+			this->start();
+		}
+	}
+	_trashesMutex.unlock();
+}
+
+bool DBDriver::getSignature(int signatureId, Signature ** s)
+{
+	*s = 0;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::map<int, Signature*>::iterator i=_trashSignatures.begin(); i!=_trashSignatures.end();)
+		{
+			if(i->first == signatureId)
+			{
+				*s = i->second;
+				_trashSignatures.erase(i++);
+				break;
+			}
+			else
+			{
+				++i;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(*s == 0)
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadQuery(signatureId, s);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+
+	return true;
+}
+
+bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
+{
+	*vw = 0;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::map<int, VisualWord*>::iterator i=_trashVisualWords.begin(); i!=_trashVisualWords.end(); ++i)
+		{
+			if((*i).first == wordId)
+			{
+				*vw = (*i).second;
+				_trashVisualWords.erase(i);
+				break;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(*vw == 0)
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadQuery(wordId, vw);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+
+	return true;
+}
+
+//Automatically begin and commit a transaction
+bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
+{
+	ULOGGER_DEBUG("");
+	std::list<KeypointSignature *> toSaveK;
+	std::list<Signature *> toUpdate;
+	if(this->isConnected() && signatures.size())
+	{
+		for(std::vector<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end();++i)
+		{
+			if((*i)->isSaved())
+			{
+				toUpdate.push_back(*i);
+			}
+			else if((*i)->signatureType().compare("KeypointSignature") == 0)
+			{
+				toSaveK.push_back((KeypointSignature *)(*i));
+			}
+			else
+			{
+				ULOGGER_ERROR("Unknown signature type ?!?");
+			}
+		}
+
+		if(toUpdate.size())
+		{
+			this->updateQuery(toUpdate);
+		}
+		if(toSaveK.size())
+		{
+			this->saveQuery(toSaveK);
+		}
+	}
+	return false;
+}
+
+bool DBDriver::load(VWDictionary * dictionary) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadQuery(dictionary);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadLastSignaturesQuery(signatures);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, bool onlyParents)
+{
+	UDEBUG("");
+	// look up in the trash before the database
+	std::list<int> ids = signIds;
+	std::list<Signature*>::iterator sIter;
+	bool valueFound = false;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
+		{
+			valueFound = false;
+			for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
+			{
+				if(sIter->first == *iter)
+				{
+					if((onlyParents && sIter->second->getLoopClosureId() == 0) || !onlyParents)
+					{
+						signatures.push_back(sIter->second);
+						_trashSignatures.erase(sIter++);
+					}
+					else
+					{
+						++sIter;
+					}
+					valueFound = true;
+					break;
+				}
+				else
+				{
+					++sIter;
+				}
+			}
+			if(valueFound)
+			{
+				iter = ids.erase(iter);
+			}
+			else
+			{
+				++iter;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+	UDEBUG("");
+	if(ids.size())
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadKeypointSignaturesQuery(ids, signatures, onlyParents);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	else if(signatures.size())
+	{
+		return true;
+	}
+	return false;
+}
+
+bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws)
+{
+	if(!wordIds.size())
+	{
+		return false;
+	}
+	// look up in the trash before the database
+	std::list<int> ids = wordIds;
+	std::map<int, VisualWord*>::iterator wIter;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
+		{
+			wIter = _trashVisualWords.find(*iter);
+			if(wIter != _trashVisualWords.end())
+			{
+				//UDEBUG("put back word %d from trash", *iter);
+				vws.push_back(wIter->second);
+				_trashVisualWords.erase(wIter);
+				iter = ids.erase(iter);
+			}
+			else
+			{
+				++iter;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+	if(ids.size())
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadWordsQuery(ids, vws);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	else if(vws.size())
+	{
+		return true;
+	}
+	return false;
+}
+
+// <oldWordId, activeWordId>
+bool DBDriver::changeWordsRef(const std::map<int, int> & refsToChange)
+{
+	//Change references in the trash
+	KeypointSignature * s = 0;
+	UTimer timer;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		timer.start();
+		for(std::map<int, Signature *>::iterator iter = _trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
+		{
+			s = dynamic_cast<KeypointSignature*>(iter->second);
+			if(s)
+			{
+				for(std::map<int, int>::const_iterator jter = refsToChange.begin(); jter!=refsToChange.end(); ++jter)
+				{
+					s->changeWordsRef((*jter).first, (*jter).second);
+				}
+			}
+		}
+		ULOGGER_DEBUG("Trash changing words references time=%fs", timer.ticks());
+	}
+	_trashesMutex.unlock();
+
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->changeWordsRefQuery(refsToChange);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::deleteWords(const std::vector<int> & ids)
+{
+	//Delete words in the trash
+	std::map<int, VisualWord*>::iterator iter;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(unsigned int i=0; i<ids.size(); ++i)
+		{
+			iter = _trashVisualWords.find(ids[i]);
+			if(iter != _trashVisualWords.end())
+			{
+				_trashVisualWords.erase(iter);
+				delete (*iter).second;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->deleteWordsQuery(ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::deleteAllVisualWords() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query;
+		query += "DELETE FROM VisualWord;";
+
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::deleteAllObsoleteSSVWLinks() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query;
+		query += "DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);";
+
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::deleteUnreferencedWords() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query = "DELETE FROM visualword WHERE id NOT IN (SELECT visualWordid FROM map_ss_vw);";
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates)
+{
+	bool r = false;
+	Signature * s = 0;
+	_trashesMutex.lock();
+	s = uValue(_trashSignatures, id, s);
+	if(s)
+	{
+		s->addNeighbor(neighbor, actuatorStates);
+		r = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!r)
+	{
+		_dbSafeAccessMutex.lock();
+		r = this->addNeighborQuery(id, neighbor, actuatorStates);
+		_dbSafeAccessMutex.unlock();
+	}
+
+	return r;
+}
+
+bool DBDriver::removeNeighbor(int id, int neighbor)
+{
+	bool r = false;
+	Signature * s = 0;
+	_trashesMutex.lock();
+	s = uValue(_trashSignatures, id, s);
+	if(s)
+	{
+		s->removeNeighbor(neighbor);
+		r = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!r)
+	{
+		r = executeNoResult("DELETE FROM Neighbor WHERE sid=" + uNumber2str(id) + " AND nid=" + uNumber2str(neighbor));
+	}
+
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getImage(int id, IplImage ** img) const
+{
+	CvMat * compressed = 0;
+	_dbSafeAccessMutex.lock();
+	bool result = this->getImageCompressedQuery(id, &compressed);
+	if(compressed)
+	{
+		(*img) = cvDecodeImage(compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		cvReleaseMat(&compressed);
+	}
+	_dbSafeAccessMutex.unlock();
+	return result;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getNeighborIdsQuery(signatureId, neighbors);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadNeighborsQuery(signatureId, neighbors);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getWeight(int signatureId, int & weight) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getWeightQuery(signatureId, weight);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLoopClosureId(int signatureId, int & loopId) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLoopClosureIdQuery(signatureId, loopId);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getImageCompressed(int id, CvMat ** compressed) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getImageCompressedQuery(id, compressed);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getAllSignatureIds(std::set<int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getAllSignatureIdsQuery(ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLastSignatureId(int & id) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLastSignatureIdQuery(id);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLastVisualWordId(int & id) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLastVisualWordIdQuery(id);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getSurfNi(int signatureId, int & ni) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getSurfNiQuery(signatureId, ni);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getChildrenIds(int signatureId, std::list<int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getChildrenIdsQuery(signatureId, ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getHighestWeightedSignaturesQuery(count, ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::stringstream query;
+		query << "INSERT INTO StatisticsAfterRun(stMemSize,lastSignAdded,processMemUsed,databaseMemUsed) values("
+			  << stMemSize << ","
+		      << lastSignAdded << ","
+		      << processMemUsed << ","
+			  << databaseMemUsed << ");";
+
+		bool r = this->executeNoResultQuery(query.str());
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::stringstream query;
+		query << "INSERT INTO StatisticsAfterRunSurf(dictionarySize) values(" << dictionarySize << ");";
+
+		bool r = this->executeNoResultQuery(query.str());
+		return r;
+	}
+	return false;
+}
+
+} // namespace rtabmap

corelib/src/DBDriverFactory.cpp

@@ -0,0 +1,70 @@
+/*
+ * 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 "rtabmap/core/DBDriverFactory.h"
+#include "DBDriverSqlite3.h"
+#include "utilite/ULogger.h"
+
+namespace rtabmap {
+
+DBDriver * DBDriverFactory::createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters)
+{
+	// TODO Do it with dynamic link libraries...
+	// Find the driver...
+	// Link dynamically to the driver...
+
+	DBDriver * driver = 0;
+
+	// Static link
+	if(dbDriverName.compare("sqlite3") == 0)
+	{
+		driver = new DBDriverSqlite3(parameters);
+	}
+	else if(dbDriverName.compare("mysql") == 0)
+	{
+		// TODO mysql driver
+		ULOGGER_ERROR("mysql driver is not implemented!");
+	}
+	else if(dbDriverName.compare("postgresql") == 0)
+	{
+		// TODO postgresql driver
+		ULOGGER_ERROR("postgresql driver is not implemented!");
+	}
+	else if(dbDriverName.compare("oracle") == 0)
+	{
+		// TODO oracle driver
+		ULOGGER_ERROR("oracle driver is not implemented!");
+	}
+	else
+	{
+		ULOGGER_ERROR("Unknown driver \"%s\"", dbDriverName.c_str());
+	}
+
+	return driver;
+}
+
+DBDriverFactory::DBDriverFactory() {
+
+}
+
+DBDriverFactory::~DBDriverFactory() {
+}
+
+}
+

corelib/src/DBDriverSqlite3.h

@@ -0,0 +1,90 @@
+/*
+ * 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 DBDRIVERSQLITE3_H_
+#define DBDRIVERSQLITE3_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/DBDriver.h"
+
+#include <sqlite3.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP DBDriverSqlite3: public DBDriver {
+public:
+	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
+	virtual ~DBDriverSqlite3();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	void setDbInMemory(bool dbInMemory);
+	void setJournalMode(int journalMode);
+	void setCacheSize(unsigned int cacheSize);
+
+private:
+	virtual bool connectDatabaseQuery(const std::string & url);
+	virtual void disconnectDatabaseQuery();
+	virtual bool isConnectedQuery() const;
+	virtual long getMemoryUsedQuery() const; // In bytes
+
+	virtual bool executeNoResultQuery(const std::string & sql) const;
+
+	virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const; // <oldWordId, activeWordId>
+	virtual bool deleteWordsQuery(const std::vector<int> & ids) const;
+	virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const;
+	virtual bool getWeightQuery(int signatureId, int & weight) const;
+	virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const;
+	virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const;
+
+	virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const;
+	virtual bool updateQuery(const std::list<Signature *> & signatures) const;
+	virtual bool saveQuery(const KeypointSignature * ss) const;
+	virtual bool saveQuery(const std::list<KeypointSignature *> & signatures) const;
+
+	// Load objects
+	virtual bool loadQuery(VWDictionary * dictionary) const;
+	virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const;
+	virtual bool loadQuery(int signatureId, Signature ** s) const;
+	virtual bool loadQuery(int wordId, VisualWord ** vw) const;
+	virtual bool loadQuery(int signatureId, KeypointSignature * ss) const;
+	virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false) const;
+	virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const;
+	virtual bool loadNeighborsQuery(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const;
+
+	virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const;
+	virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const;
+	virtual bool getLastSignatureIdQuery(int & id) const;
+	virtual bool getLastVisualWordIdQuery(int & id) const;
+	virtual bool getSurfNiQuery(int signatureId, int & ni) const;
+	virtual bool getChildrenIdsQuery(int signatureId, std::list<int> & ids) const;
+	virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int, int> & ids) const;
+
+private:
+	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
+
+private:
+	sqlite3 * _ppDb;
+	bool _dbInMemory;
+	unsigned int _cacheSize;
+	int _journalMode;
+};
+
+}
+
+#endif /* DBDRIVERSQLITE3_H_ */

corelib/src/EpipolarGeometry.cpp

@@ -0,0 +1,111 @@
+/*
+ * 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 "rtabmap/core/EpipolarGeometry.h"
+#include "utilite/ULogger.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/core_c.h>
+
+namespace rtabmap
+{
+
+//Epipolar geometry
+void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
+{
+	if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
+	{
+		ULOGGER_ERROR("The matrix is not the good size...");
+		return;
+	}
+
+	if(fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_ERROR("The matrix is not the good type...");
+		return;
+	}
+
+	CvMat * w = cvCreateMat(3, 3, CV_64FC1);
+	CvMat * u = cvCreateMat(3, 3, CV_64FC1);
+	CvMat * v = cvCreateMat(3, 3, CV_64FC1);
+
+	CvMat f = fundamentalMatrix;
+	cvSVD(&f, w, u, v);
+
+	// v is for image 1
+	// u is for image 2
+
+	e1[0] = v->data.db[0*3+2];// /v->data.db[2*3+2];
+	e1[1] = v->data.db[1*3+2];// /v->data.db[2*3+2];
+	e1[2] = v->data.db[2*3+2];// /v->data.db[2*3+2];
+
+	e2[0] = u->data.db[0*3+2];// /u->data.db[2*3+2];
+	e2[1] = u->data.db[1*3+2];// /u->data.db[2*3+2];
+	e2[2] = u->data.db[2*3+2];// /u->data.db[2*3+2];
+
+	cvReleaseMat(&w);
+	cvReleaseMat(&u);
+	cvReleaseMat(&v);
+}
+
+// P2 = [M | t] = [[e']_x * F | e']
+void findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2)
+{
+	if(p2.rows != 3 || p2.cols != 4 || fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
+	{
+		ULOGGER_ERROR("Matrices are not the good size... ");
+		return;
+	}
+
+	if(p2.type()!= CV_64FC1 || fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_ERROR("Matrices are not the good type...");
+		return;
+	}
+
+	if(e2[0] == 0 && e2[1] == 0 && e2[2] == 0)
+	{
+		cv::Vec3d e1;
+		findEpipolesFromF(fundamentalMatrix, e1, e2);
+	}
+
+	double e2_sd[3*3] = { 0., -e2[2], e2[1],
+						   e2[2], 0., -e2[0],
+						  -e2[1], e2[0], 0. };
+	CvMat e2_smt = cvMat( 3, 3, CV_64FC1, e2_sd );
+	cv::Mat e2_sm(&e2_smt); //;
+
+	cv::Mat m = e2_sm*fundamentalMatrix;
+
+	p2.at<double>(0,0) = m.at<double>(0,0);
+	p2.at<double>(0,1) = m.at<double>(0,1);
+	p2.at<double>(0,2) = m.at<double>(0,2);
+	p2.at<double>(1,0) = m.at<double>(1,0);
+	p2.at<double>(1,1) = m.at<double>(1,1);
+	p2.at<double>(1,2) = m.at<double>(1,2);
+	p2.at<double>(2,0) = m.at<double>(2,0);
+	p2.at<double>(2,1) = m.at<double>(2,1);
+	p2.at<double>(2,2) = m.at<double>(2,2);
+
+	p2.at<double>(0,3) = e2[0];
+	p2.at<double>(1,3) = e2[1];
+	p2.at<double>(2,3) = e2[2];
+}
+
+} // namespace rtabmap

corelib/src/KeypointDescriptor.cpp

@@ -0,0 +1,541 @@
+/*
+ * 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 "rtabmap/core/KeypointDescriptor.h"
+#include "utilite/UStl.h"
+#include "utilite/UConversion.h"
+#include "utilite/ULogger.h"
+#include "utilite/UMath.h"
+#include "utilite/ULogger.h"
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/gpu/gpu.hpp>
+#include <opencv2/core/version.hpp>
+
+#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
+
+namespace rtabmap {
+
+KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	_childDescriptor(childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+KeypointDescriptor::~KeypointDescriptor()
+{
+	if(_childDescriptor)
+	{
+		delete _childDescriptor;
+	}
+}
+
+void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	if(_childDescriptor)
+	{
+		_childDescriptor->parseParameters(parameters);
+	}
+}
+
+std::list<std::vector<float> > KeypointDescriptor::generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	// see decorator pattern...
+	std::list<std::vector<float> > descriptors = this->_generateDescriptors(image, keypoints);
+	std::list<std::vector<float> > childDescriptors;
+	if(_childDescriptor)
+	{
+		childDescriptors = _childDescriptor->generateDescriptors(image, keypoints);
+
+		if(childDescriptors.size() && childDescriptors.size() == descriptors.size())
+		{
+			std::list<std::vector<float> >::iterator iterDesc = descriptors.begin();
+			std::list<std::vector<float> >::iterator iterChild = childDescriptors.begin();
+			for(; iterDesc!=descriptors.end(); ++iterDesc, ++iterChild)
+			{
+				iterDesc->insert(iterDesc->end(), iterChild->begin(), iterChild->end());
+			}
+		}
+	}
+	return descriptors;
+}
+
+void KeypointDescriptor::setChildDescriptor(KeypointDescriptor * childDescriptor)
+{
+	if(_childDescriptor)
+	{
+		delete _childDescriptor;
+	}
+	_childDescriptor = childDescriptor;
+}
+
+//////////////////////////
+//SURFDescriptor
+//////////////////////////
+SURFDescriptor::SURFDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_surf.extended = Parameters::defaultSURFExtended();
+	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_gpuVersion = Parameters::defaultSURFGpuVersion();
+	_upright = Parameters::defaultSURFUpright();
+	this->parseParameters(parameters);
+}
+
+SURFDescriptor::~SURFDescriptor()
+{
+}
+
+void SURFDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
+	{
+		_surf.extended = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
+	{
+		_surf.hessianThreshold = std::atof((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
+	{
+		_surf.nOctaveLayers = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
+	{
+		_gpuVersion = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
+	{
+		_upright = uStr2Bool((*iter).second.c_str());
+	}
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+	cv::Mat mask;
+	std::vector<cv::KeyPoint> k = uListToVector(keypoints);
+	std::vector<float> d;
+#if OPENCV_SURF_GPU
+	if(_gpuVersion)
+	{
+		cv::gpu::GpuMat imgGpu(img);
+		cv::gpu::GpuMat descriptorsGpu;
+		cv::gpu::GpuMat keypointsGpu;
+		cv::gpu::SURF_GPU surfGpu(_surf.hessianThreshold, _surf.nOctaves, _surf.nOctaveLayers, _surf.extended, 0.01f, _upright);
+		surfGpu.uploadKeypoints(k, keypointsGpu);
+		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
+		surfGpu.downloadDescriptors(descriptorsGpu, d);
+	}
+	else
+	{
+		_surf(img, mask, k, d, true); // Opencv surf descriptors
+	}
+#else
+	_surf(img, mask, k, d, true); // Opencv surf descriptors
+#endif
+
+	unsigned int dim = _surf.descriptorSize();
+	for(unsigned int i=0; i<d.size(); i+=dim)
+	{
+		descriptors.push_back(std::vector<float>(d.begin()+i, d.begin()+i+dim));
+	}
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//SIFTDescriptor
+//////////////////////////
+SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+SIFTDescriptor::~SIFTDescriptor()
+{
+}
+
+void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+	cv::Mat mask;
+	std::vector<cv::KeyPoint> k = uListToVector(keypoints);
+	cv::Mat d;
+	cv::SIFT sift(_commonParams, cv::SIFT::DetectorParams(), _descriptorParams);
+	sift(img, mask, k, d, true); // Opencv surf descriptors
+	unsigned int dim = sift.descriptorSize();
+	//ULOGGER_DEBUG("row=%d, col=%d, type=%d (float=%d)", d.rows, d.cols, d.type(), CV_32F);
+	for(int i=0; i<d.rows; ++i)
+	{
+		descriptors.push_back(std::vector<float>(d.ptr<float>(i), d.ptr<float>(i)+dim));
+	}
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//LaplacianDescriptor
+//////////////////////////
+LaplacianDescriptor::LaplacianDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+LaplacianDescriptor::~LaplacianDescriptor()
+{
+}
+
+void LaplacianDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+		std::vector<float> laplacian(1);
+		laplacian[0] = uSign(key->response);
+		descriptors.push_back(laplacian);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//ColorDescriptor
+//////////////////////////
+ColorDescriptor::ColorDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+ColorDescriptor::~ColorDescriptor()
+{
+}
+
+void ColorDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+
+	IplImage * imageConverted = 0;
+	if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
+	{
+		imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
+		cvCvtColor(image, imageConverted, CV_GRAY2BGR);
+	}
+	cv::Mat imgMat;
+	if(imageConverted)
+	{
+		imgMat = cv::Mat(imageConverted);
+	}
+	else
+	{
+		imgMat =  cv::Mat(image);
+	}
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+
+		int grayMax = -1;  // grayValue
+		int grayMin = -1;  // grayValue
+		float d[6] = {0};
+		std::vector<int> RxV;
+		cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
+		int R = cvRound(key->size*1.2/9.*2);
+		this->getCircularROI(R, RxV);
+		cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
+		// find the brighter and darker pixels
+		for( int dy = -R; dy <= R; ++dy )
+		{
+			int Rx = RxV[abs(dy)];
+			for( int dx = -Rx; dx <= Rx; ++dx )
+			{
+				if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
+				{
+					//bgr
+					uchar b = img(center.y+dy, center.x+dx)[0];
+					uchar g = img(center.y+dy, center.x+dx)[1];
+					uchar r = img(center.y+dy, center.x+dx)[2];
+					int gray = b*0.114 + g*0.587 + r*0.299;
+					if(grayMax<0 || gray > grayMax)
+					{
+						grayMax = gray;
+						d[0] = b;
+						d[1] = g;
+						d[2] = r;
+					}
+					if(grayMin<0 || gray < grayMin)
+					{
+						grayMin = gray;
+						d[3] = b;
+						d[4] = g;
+						d[5] = r;
+					}
+				}
+				else
+				{
+					//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
+				}
+			}
+		}
+		for(int i=0; i<6; ++i)
+		{
+			d[i] /= 255; // Normalize between 0 and 1
+		}
+		descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
+	}
+
+	if(imageConverted)
+	{
+		cvReleaseImage(&imageConverted);
+	}
+
+	return descriptors;
+}
+
+// the function returns x boundary coordinates of
+// the circle for each y. RxV[y1] = x1 means that
+// when y=y1, -x1 <=x<=x1 is inside the circle
+// (from OpenCv doc, C++ Cheatsheet)
+void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
+{
+    RxV.resize(R+1);
+    for( int y = 0; y <= R; y++ )
+        RxV[y] = cvRound(sqrt(double(R*R - y*y)));
+}
+
+//////////////////////////
+//HueDescriptor
+//////////////////////////
+HueDescriptor::HueDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+		ColorDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+HueDescriptor::~HueDescriptor()
+{
+}
+
+void HueDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+
+	IplImage * imageConverted = 0;
+	if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
+	{
+		imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
+		cvCvtColor(image, imageConverted, CV_GRAY2BGR);
+	}
+	cv::Mat imgMat;
+	if(imageConverted)
+	{
+		imgMat = cv::Mat(imageConverted);
+	}
+	else
+	{
+		imgMat =  cv::Mat(image);
+	}
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+
+		int intensityMax = -1;
+		int intensityMin = -1;
+		float d[2] = {0};
+		std::vector<int> RxV;
+		cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
+		int R = cvRound(key->size*1.2/9.*2);
+		this->getCircularROI(R, RxV);
+		cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
+		// find the brighter and darker pixels using the intensity
+		int dxb=0;
+		int dyb=0;
+		int dxd=0;
+		int dyd=0;
+		for( int dy = -R; dy <= R; ++dy )
+		{
+			int Rx = RxV[abs(dy)];
+			for( int dx = -Rx; dx <= Rx; ++dx )
+			{
+				if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
+				{
+					//bgr
+					float b = float(img(center.y+dy, center.x+dx)[0]) / 255.0f;
+					float g = float(img(center.y+dy, center.x+dx)[1]) / 255.0f;
+					float r = float(img(center.y+dy, center.x+dx)[2]) / 255.0f;
+					int intensity = rgb2intensity(r, g, b);
+					if(intensityMax<0 || intensity > intensityMax)
+					{
+						intensityMax = intensity;
+						dxb = dx;
+						dyb = dy;
+					}
+					if(intensityMin<0 || intensity < intensityMin)
+					{
+						intensityMin = intensity;
+						dxd = dx;
+						dyd = dy;
+					}
+				}
+				else
+				{
+					//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
+				}
+			}
+		}
+
+		// brighter
+		float b = float(img(center.y+dyb, center.x+dxb)[0]) / 255.0f;
+		float g = float(img(center.y+dyb, center.x+dxb)[1]) / 255.0f;
+		float r = float(img(center.y+dyb, center.x+dxb)[2]) / 255.0f;
+		d[0] = rgb2hue(r, g, b);
+
+		// darker
+		b = float(img(center.y+dyd, center.x+dxd)[0]) / 255.0f;
+		g = float(img(center.y+dyd, center.x+dxd)[1]) / 255.0f;
+		r = float(img(center.y+dyd, center.x+dxd)[2]) / 255.0f;
+		d[1] = rgb2hue(r, g, b);
+
+		descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
+	}
+
+	if(imageConverted)
+	{
+		cvReleaseImage(&imageConverted);
+	}
+
+	return descriptors;
+}
+
+// assuming that rgb values are normalized [0,1]
+float HueDescriptor::rgb2hue(float r, float g, float b) const
+{
+	double pi = 3.14159265359;
+	if(b<=g)
+	{
+		return acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b))))/pi;
+	}
+	else
+	{
+		return (pi-acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b)))))/pi;
+	}
+}
+
+
+}

corelib/src/KeypointDetector.cpp

@@ -0,0 +1,496 @@
+/*
+ * 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 "rtabmap/core/KeypointDetector.h"
+#include "VWDictionary.h"
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UStl.h"
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/gpu/gpu.hpp>
+#include <opencv2/core/version.hpp>
+
+#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
+
+namespace rtabmap
+{
+
+KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
+		_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
+		_usingAdaptiveResponseThr(Parameters::defaultKpUsingAdaptiveResponseThr()),
+		_adaptiveResponseThr(1),
+		_roiRatios(std::vector<float>(4, 0.0f))
+{
+	this->setRoi(Parameters::defaultKpRoiRatios());
+	this->parseParameters(parameters);
+}
+
+void KeypointDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
+	{
+		_wordsPerImageTarget = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kKpUsingAdaptiveResponseThr())) != parameters.end())
+	{
+		_usingAdaptiveResponseThr = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
+	{
+		this->setRoi((*iter).second);
+	}
+}
+
+std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(image)
+	{
+		UTimer timer;
+		timer.start();
+
+		cv::Rect roi = computeRoi(image);
+
+		// Get keypoints
+		keypoints = this->_generateKeypoints(image, roi);
+		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
+
+		//clip the number of words... to _wordsPerImageTarget
+		// Variable hessian threshold
+		if(_wordsPerImageTarget > 0)
+		{
+			ULOGGER_DEBUG("_adaptiveResponseThr=%f", _adaptiveResponseThr);
+			if(keypoints.size() > 0)
+			{
+				if(keypoints.size() > _wordsPerImageTarget)
+				{
+					_adaptiveResponseThr *= 1+((float(keypoints.size())/float(_wordsPerImageTarget)-1)/1000);
+				}
+				else if(keypoints.size() < _wordsPerImageTarget)
+				{
+					_adaptiveResponseThr *= 1-((1-float(keypoints.size())/float(_wordsPerImageTarget))/1);
+				}
+				// 10% margin...
+				if(keypoints.size() > 1.1 * _wordsPerImageTarget)
+				{
+					ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
+					// Remove words under the new hessian threshold
+
+					// Sort words by hessian
+					std::multimap<float, std::list<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
+					for(std::list<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
+					{
+						//Keep track of the data, to be easier to manage the data in the next step
+						hessianMap.insert(std::pair<float, std::list<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
+					}
+
+					// Remove them from the signature
+					int removed = 0;
+					unsigned int stopIndex = hessianMap.size()-_wordsPerImageTarget;
+					std::multimap<float, std::list<cv::KeyPoint>::iterator>::iterator iter = hessianMap.begin();
+					for(unsigned int k=0; k < stopIndex && iter!=hessianMap.end(); ++k, ++iter)
+					{
+						keypoints.erase(iter->second);
+						++removed;
+					}
+					if(iter->first!=0)
+					{
+						_adaptiveResponseThr = iter->first;
+					}
+					ULOGGER_DEBUG("%d keypoints removed, (kept %d)", removed, keypoints.size());
+				}
+			}
+			else
+			{
+				_adaptiveResponseThr /= 2;
+			}
+
+			if(_adaptiveResponseThr < this->getMinimumResponseThr())
+			{
+				_adaptiveResponseThr = this->getMinimumResponseThr();
+			}
+
+			ULOGGER_DEBUG("new _adaptiveResponseThr=%f", _adaptiveResponseThr);
+			ULOGGER_DEBUG("adjusting hessian threshold time = %f s", timer.ticks());
+		}
+
+		// Adjust keypoint position to raw image
+		for(std::list<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+		{
+			iter->pt.x += roi.x;
+			iter->pt.y += roi.y;
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Image is null!");
+	}
+	return keypoints;
+}
+
+void KeypointDetector::setRoi(const std::string & roi)
+{
+	std::list<std::string> strValues = uSplit(roi, ' ');
+	if(strValues.size() != 4)
+	{
+		ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
+	}
+	else
+	{
+		std::vector<float> tmpValues(4);
+		unsigned int i=0;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			tmpValues[i] = std::atof((*iter).c_str());
+			++i;
+		}
+
+		if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+			tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+			tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+			tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+		{
+			_roiRatios = tmpValues;
+		}
+		else
+		{
+			ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
+		}
+	}
+}
+
+cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
+{
+	if(image && _roiRatios.size() == 4)
+	{
+		cv::Rect roi(0, 0, image->width, image->height);
+		UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		float width = image->width;
+		float height = image->height;
+		//left roi
+		if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
+		{
+			roi.x = width * _roiRatios[0];
+		}
+
+		//right roi
+		roi.width = width - roi.x;
+		if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
+		{
+			roi.width -= width * _roiRatios[1];
+		}
+
+		//top roi
+		if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
+		{
+			roi.y = height * _roiRatios[2];
+		}
+
+		//bottom roi
+		roi.height = height - roi.y;
+		if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
+		{
+			roi.height -= height * _roiRatios[3];
+		}
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		return roi;
+	}
+	else
+	{
+		UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
+		return cv::Rect();
+	}
+}
+
+
+//////////////////////////
+//SURFDetector
+//////////////////////////
+SURFDetector::SURFDetector(const ParametersMap & parameters) :
+		KeypointDetector(parameters)
+{
+	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_surf.extended = Parameters::defaultSURFExtended();
+	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_gpuVersion = Parameters::defaultSURFGpuVersion();
+	_upright = Parameters::defaultSURFUpright();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_surf.hessianThreshold);
+}
+
+SURFDetector::~SURFDetector()
+{
+}
+
+void SURFDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
+	{
+		_surf.extended = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
+	{
+		_surf.hessianThreshold = std::atof((*iter).second.c_str());
+		this->setAdaptiveResponseThr(_surf.hessianThreshold);
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
+	{
+		_surf.nOctaveLayers = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
+	{
+		_gpuVersion = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
+	{
+		_upright = uStr2Bool((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+
+	cv::SURF surf = _surf;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		surf.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+#if OPENCV_SURF_GPU
+	if(_gpuVersion )
+	{
+		cv::gpu::GpuMat imgGpu(imgRoi);
+		cv::gpu::GpuMat keypointsGpu;
+		cv::gpu::SURF_GPU surfGpu(surf.hessianThreshold, surf.nOctaves, surf.nOctaveLayers, surf.extended, 0.01f, _upright);
+		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
+		surfGpu.downloadKeypoints(keypointsGpu, k);
+	}
+	else
+	{
+		surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+	}
+#else
+	surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+#endif
+	keypoints = uVectorToList(k);
+
+
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return keypoints;
+}
+
+//////////////////////////
+//SIFTDetector
+//////////////////////////
+SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
+		KeypointDetector(parameters)
+{
+	_detectorParams.threshold = Parameters::defaultSIFTThreshold();
+	_detectorParams.edgeThreshold = Parameters::defaultSIFTEdgeThreshold();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_detectorParams.threshold);
+}
+
+SIFTDetector::~SIFTDetector()
+{
+}
+
+void SIFTDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSIFTThreshold())) != parameters.end())
+	{
+		_detectorParams.threshold = std::atof((*iter).second.c_str());
+		this->setAdaptiveResponseThr(_detectorParams.threshold);
+	}
+	if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
+	{
+		_detectorParams.edgeThreshold = std::atof((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+
+	cv::SIFT::DetectorParams detectorParam = _detectorParams;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		detectorParam.threshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+	cv::Mat mask;
+	cv::SIFT sift(_commonParams, detectorParam);
+
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+	sift(imgRoi, mask, k); // Opencv surf keypoints
+	keypoints = uVectorToList(k);
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return keypoints;
+}
+
+
+//////////////////////////
+//StarDetector
+//////////////////////////
+StarDetector::StarDetector(const ParametersMap & parameters) :
+	KeypointDetector(parameters)
+{
+	_star.lineThresholdBinarized = Parameters::defaultStarLineThresholdBinarized();
+	_star.lineThresholdProjected = Parameters::defaultStarLineThresholdProjected();
+	_star.maxSize = Parameters::defaultStarMaxSize();
+	_star.responseThreshold = Parameters::defaultStarResponseThreshold();
+	_star.suppressNonmaxSize = Parameters::defaultStarSuppressNonmaxSize();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_star.responseThreshold);
+}
+
+StarDetector::~StarDetector()
+{
+}
+
+void StarDetector::parseParameters(const ParametersMap & parameters)
+{
+	ULOGGER_WARN("The StarDetector parameters can't be changed on ROS (this is an issue with the default (and too old) opencv revision used in ROS)");
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
+	{
+		_star.lineThresholdBinarized = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
+	{
+		_star.lineThresholdProjected = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
+	{
+		_star.maxSize = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
+	{
+		_star.responseThreshold = int(std::atof((*iter).second.c_str()));
+		this->setAdaptiveResponseThr(_star.responseThreshold);
+	}
+	if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
+	{
+		_star.suppressNonmaxSize = std::atoi((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+
+	cv::Mat img(image);
+	cv::Mat mask;
+	// TODO More testing needed with the star detector, NN search distance must be changed to 0.8
+	//find keypoints with the star detector
+	cv::StarDetector star = _star;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		star.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+
+	// Get keypoints with the star detector
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+	star(imgRoi, k);
+	keypoints = uVectorToList(k);
+	return keypoints;
+}
+
+}

corelib/src/KeypointMemory.h

@@ -0,0 +1,89 @@
+/*
+ * 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 KEYPOINTMEMORY_H_
+#define KEYPOINTMEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "Memory.h"
+
+namespace rtabmap {
+
+class VWDictionary;
+class VisualWord;
+class KeypointDetector;
+class KeypointDescriptor;
+
+class RTABMAP_EXP KeypointMemory : public Memory
+{
+public:
+	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
+	enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
+
+public:
+	KeypointMemory(const ParametersMap & parameters = ParametersMap());
+	virtual ~KeypointMemory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
+	virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
+	virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
+	virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
+	virtual void dumpMemory(std::string directory) const;
+	virtual void dumpSignatures(const char * fileNameSign) const;
+
+	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+
+	const KeypointDetector * getKeypointDetector() const {return _keypointDetector;}
+	const KeypointDescriptor * getKeypointDescriptor() const {return _keypointDescriptor;}
+	const VWDictionary * getVWD() const {return _vwd;}
+	DetectorStrategy detectorStrategy() const;
+
+protected:
+	virtual Signature * getSignatureLtMem(int id);
+	virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual void preUpdate();
+	virtual void merge(const Signature * from, Signature * to, MergingStrategy s);
+
+private:
+	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
+	void disableWordsRef(int signatureId);
+	void enableWordsRef(const std::list<int> & signatureIds);
+	void cleanUnusedWords();
+	int getNi(int signatureId) const;
+
+private:
+	std::list<int> _commonWords;
+	VWDictionary * _vwd;
+	KeypointDetector * _keypointDetector;
+	KeypointDescriptor * _keypointDescriptor;
+	//std::map<int, int> _wordRefsToChange;
+	bool _reactivatedWordsComparedToNewWords;
+	float _badSignRatio;;
+	bool _tfIdfLikelihoodUsed;
+	bool _parallelized;
+	bool _tfIdfNormalized;
+};
+
+}
+
+#endif /* KEYPOINTMEMORY_H_ */

corelib/src/Memory.h

@@ -0,0 +1,160 @@
+/*
+ * 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 MEMORY_H_
+#define MEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UVariant.h"
+#include <typeinfo>
+#include <list>
+#include <map>
+#include <set>
+#include "utilite/UStl.h"
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class Signature;
+class DBDriver;
+class Node;
+class SMState;
+
+class RTABMAP_EXP Memory
+{
+public:
+	static const int kIdStart;
+	static const int kIdVirtual;
+	static const int kIdInvalid;
+
+	enum MergingStrategy{kFullMerging, kUseOnlyFromMerging, kUseOnlyDestMerging};
+
+public:
+	Memory(const ParametersMap & parameters = ParametersMap());
+	virtual ~Memory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	bool update(const SMState * rawData, std::list<std::pair<std::string, float> > & stats);
+	virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
+	virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
+	virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
+	virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
+
+	int cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	void emptyTrash();
+	void joinTrashThread();
+	void addLoopClosureLink(int oldId, int newId, bool rehearsal = false);
+	void getNeighborsId(std::map<int,int> & ids, int signatureId, unsigned int margin, bool checkInDatabase = true, int ignoredId = 0) const;
+
+	//getters
+	unsigned int getWorkingMemSize() const {return _workingMem.size();}
+	unsigned int getStMemSize() const {return _stMem.size();};
+	const std::map<int, int> & getWorkingMem() const {return _workingMem;}
+	const std::set<int> & getStMem() const {return _stMem;}
+	std::list<int> getChildrenIds(int signatureId) const;
+	bool isRawDataKept() const {return _rawDataKept;}
+	std::map<int, int> getWeights() const;
+	int getWeight(int id) const;
+	float getSimilarityOnlyLast() const {return _similarityOnlyLast;}
+	const Signature * getLastSignature() const;
+	int getDatabaseMemoryUsed() const; // in bytes
+	double getDbSavingTime() const;
+	IplImage * getImage(int id) const;
+	bool isDatabaseCleaned()  const {return _databaseCleaned;}
+	bool isCommonSignatureUsed() const {return _commonSignatureUsed;}
+	std::set<int> getAllSignatureIds() const;
+	bool memoryChanged() const {return _memoryChanged;}
+	const Signature * getSignature(int id) const;
+	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
+	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
+	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
+
+	//setters
+	void setSimilarityThreshold(float similarityThreshold);
+	void setSimilarityOnlyLast(int similarityOnlyLast) {_similarityOnlyLast = similarityOnlyLast;}
+	void setOldSignatureRatio(float oldSignatureRatio);
+	void setMaxStMemSize(unsigned int maxStMemSize);
+	void setDelayRequired(int delayRequired);
+	void setRecentWmRatio(float recentWmRatio);
+	void setCommonSignatureUsed(bool commonSignatureUsed);
+	void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
+
+	void dumpMemoryTree(const char * fileNameTree) const;
+	virtual void dumpMemory(std::string directory) const;
+	virtual void dumpSignatures(const char * fileNameSign) const {}
+	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
+	void cleanLocalGraph(int id, unsigned int margin);
+	void cleanLTM(int maxDepth = 10);
+	void createGraph(Node * parent, unsigned int maxDepth, const std::set<int> & endIds = std::set<int>());
+
+protected:
+	virtual void preUpdate();
+	virtual void postUpdate() {}
+	virtual void merge(const Signature * from, Signature * to, MergingStrategy s) = 0;
+
+	virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual Signature * getSignatureLtMem(int id);
+
+	void addSignatureToWm(Signature * signature);
+	Signature * _getSignature(int id) const;
+	Signature * _getLastSignature();
+	Signature * getRemovableSignature(const std::list<int> & ignoredIds = std::list<int>(), bool onlyLoopedSignatures = false);
+	int getNextId();
+	void initCountId();
+	int rehearsal(const Signature * signature, bool onlyLast, float & similarity);
+	void touch(int signatureId);
+	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
+
+private:
+	void createVirtualSignature(Signature ** signature);
+	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false) = 0;
+	void cleanGraph(const Node * root);
+protected:
+	DBDriver * _dbDriver;
+
+private:
+	float _similarityThreshold;
+	bool _similarityOnlyLast;
+	bool _rawDataKept;
+	int _idCount;
+	Signature * _lastSignature;
+	int _lastLoopClosureId;
+	bool _incrementalMemory;
+	unsigned int _maxStMemSize;
+	bool _commonSignatureUsed;
+	bool _databaseCleaned; //if true, delete old signatures in the database
+	int _delayRequired;
+	float _recentWmRatio;
+	bool _memoryChanged; // False by default, become true when Memory::update() is called.
+	bool _merging;
+	int _signaturesAdded;
+
+	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
+	std::set<int> _stMem;
+	std::map<int, int> _workingMem; // id, timeStamp
+};
+
+} // namespace rtabmap
+
+#endif /* MEMORY_H_ */

corelib/src/NearestNeighbor.cpp

@@ -0,0 +1,189 @@
+/*
+ * 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 "NearestNeighbor.h"
+#include "utilite/ULogger.h"
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+/////////////////////////
+// KdTreeNN
+/////////////////////////
+KdTreeNN::KdTreeNN(const ParametersMap & parameters) :
+		_tree(0)
+{
+	ULOGGER_DEBUG("");
+	this->parseParameters(parameters);
+}
+
+KdTreeNN::~KdTreeNN()
+{
+	if(_tree)
+	{
+		cvReleaseFeatureTree(_tree);
+	}
+}
+
+void KdTreeNN::setData(const cv::Mat & data)
+{
+	if(_tree)
+	{
+		cvReleaseFeatureTree(_tree);
+		_tree = 0;
+	}
+
+	// convert to old style mat (data is not copied)
+	_dataMat = data;
+	_tree = cvCreateKDTree(&_dataMat);
+}
+
+void KdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
+{
+	ULOGGER_DEBUG("");
+	if(_tree)
+	{
+		// convert to old style mat (data is not copied)
+		CvMat queriesMat = queries;
+		CvMat indicesMat = indices;
+		CvMat distsMat = dists;
+		cvFindFeatures(_tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
+	}
+	else
+	{
+		ULOGGER_ERROR("The search tree is not created, setData() must be called first");
+	}
+}
+
+void KdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
+{
+	ULOGGER_DEBUG("");
+	CvMat dataMat = data;
+	CvFeatureTree * tree = cvCreateKDTree(&dataMat);
+
+	if(tree)
+	{
+		// convert to old style mat (data is not copied)
+		CvMat queriesMat = queries;
+		CvMat indicesMat = indices;
+		CvMat distsMat = dists;
+		cvFindFeatures(tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
+		cvReleaseFeatureTree(tree);
+	}
+	else
+	{
+		ULOGGER_ERROR("The search tree creation failed ?!?");
+	}
+}
+
+void KdTreeNN::parseParameters(const ParametersMap & parameters)
+{
+	NearestNeighbor::parseParameters(parameters);
+}
+
+
+
+
+/////////////////////////
+// FlannKdTreeNN
+/////////////////////////
+FlannKdTreeNN::FlannKdTreeNN(const ParametersMap & parameters) :
+		_treeFlannIndex(0),
+		_strategy(kKDTree)
+{
+	ULOGGER_DEBUG("");
+	this->parseParameters(parameters);
+}
+
+FlannKdTreeNN::~FlannKdTreeNN() {
+	if(_treeFlannIndex)
+	{
+		delete _treeFlannIndex;
+	}
+}
+
+void FlannKdTreeNN::setData(const cv::Mat & data)
+{
+	if(_treeFlannIndex)
+	{
+		delete _treeFlannIndex;
+		_treeFlannIndex = 0;
+	}
+
+	_treeFlannIndex = createIndex(data, _strategy);  // using 4 randomized trees
+	//_treeFlannIndex = new cv::flann::Index(_dataTree, cv::flann::AutotunedIndexParams(0.9, 0.01, 0, 0.1));  // use autotuned parameters
+}
+
+void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
+{
+	ULOGGER_DEBUG("");
+	if(_treeFlannIndex)
+	{
+		// Note, the search params is ignored because we use an autotuned created index (see update())
+		_treeFlannIndex->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
+	}
+	else
+	{
+		ULOGGER_ERROR("The search index is not created, setData() must be called first");
+	}
+}
+
+void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
+{
+	ULOGGER_DEBUG("");
+	cv::flann::Index * index = createIndex(data, _strategy);
+	// Note, the search params is ignored because we use an autotuned created index (see update())
+	index->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
+	delete index;
+}
+
+void FlannKdTreeNN::parseParameters(const ParametersMap & parameters)
+{
+	NearestNeighbor::parseParameters(parameters);
+}
+
+enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
+cv::flann::Index * FlannKdTreeNN::createIndex(const cv::Mat & data, Strategy s) const
+{
+	cv::flann::Index * index = 0;
+	switch(s)
+	{
+	case kLinear:
+		index = new cv::flann::Index(data, cv::flann::LinearIndexParams());
+		break;
+	case kKDTree:
+		index = new cv::flann::Index(data, cv::flann::KDTreeIndexParams());
+		break;
+	case kMeans:
+		index = new cv::flann::Index(data, cv::flann::KMeansIndexParams());
+		break;
+	case kComposite:
+		index = new cv::flann::Index(data, cv::flann::CompositeIndexParams());
+		break;
+	case kAutoTuned:
+	default:
+		index = new cv::flann::Index(data, cv::flann::AutotunedIndexParams());
+		break;
+	}
+	return index;
+}
+
+
+}

corelib/src/NearestNeighbor.h

@@ -0,0 +1,152 @@
+/*
+ * 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 NEARESTNEIGHBOR_H_
+#define NEARESTNEIGHBOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc_c.h>
+#include <map>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class VisualWord;
+
+class RTABMAP_EXP NearestNeighbor
+{
+public:
+
+public:
+	virtual ~NearestNeighbor() {}
+
+	virtual void setData(const cv::Mat & data) = 0;
+
+	virtual void search(
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) = 0;
+
+	virtual void search(
+			const cv::Mat & data,
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) const = 0;
+
+	virtual bool isDist64F() const = 0;
+	virtual bool isDistSquared() const = 0;
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+
+protected:
+	NearestNeighbor() {}
+};
+
+
+
+/////////////////////////
+// KdTreeNN
+/////////////////////////
+class RTABMAP_EXP KdTreeNN : public NearestNeighbor
+{
+public:
+	KdTreeNN(const ParametersMap & parameters = ParametersMap());
+	virtual ~KdTreeNN();
+
+	virtual void setData(const cv::Mat & data);
+
+	virtual void search(const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64);
+
+	virtual void search(const cv::Mat & data,
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) const;
+
+	virtual bool isDist64F() const {return true;}
+	virtual bool isDistSquared() const {return false;}
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	CvFeatureTree * _tree;
+	CvMat _dataMat;
+};
+
+
+
+
+
+
+/////////////////////////
+// FlannKdTreeNN
+/////////////////////////
+class RTABMAP_EXP FlannKdTreeNN : public NearestNeighbor
+{
+public:
+	enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
+
+public:
+	FlannKdTreeNN(const ParametersMap & parameters = ParametersMap());
+	FlannKdTreeNN(Strategy s, const ParametersMap & parameters = ParametersMap());
+	virtual ~FlannKdTreeNN();
+
+	void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
+
+	virtual void setData(const cv::Mat & data);
+
+	virtual void search(const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64);
+
+	virtual void search(const cv::Mat & data,
+				const cv::Mat & queries,
+				cv::Mat & indices,
+				cv::Mat & dists,
+				int knn = 1,
+				int emax = 64) const;
+
+	virtual bool isDist64F() const {return false;}
+	virtual bool isDistSquared() const {return true;}
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	cv::flann::Index * createIndex(const cv::Mat & data, Strategy s) const;
+
+private:
+	cv::flann::Index * _treeFlannIndex;
+	Strategy _strategy;
+};
+
+}
+
+#endif /* NEARESTNEIGHBOR_H_ */

corelib/src/Node.h

@@ -0,0 +1,98 @@
+/*
+ * 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 NODE_H_
+#define NODE_H_
+
+namespace rtabmap {
+
+class Node
+{
+public:
+	Node(int id, Node * parent = 0) :
+		_parent(parent),
+		_id(id)
+	{
+		if(_parent)
+		{
+			_parent->addChild(this);
+		}
+	}
+	virtual ~Node()
+	{
+		//We copy the set because when a child is destroyed, it is removed from its parent.
+		std::set<Node*> children = _children;
+		_children.clear();
+		for(std::set<Node*>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+		{
+			delete *iter;
+		}
+		children.clear();
+		if(_parent)
+		{
+			_parent->removeChild(this);
+		}
+	}
+	int id() const {return _id;}
+	bool isAncestor(int id) const
+	{
+		if(_parent)
+		{
+			if(_parent->id() == id)
+			{
+				return true;
+			}
+			return _parent->isAncestor(id);
+		}
+		return false;
+	}
+
+	void expand(std::list<std::list<int> > & paths, std::list<int> currentPath = std::list<int>()) const
+	{
+		currentPath.push_back(_id);
+		if(_children.size() == 0)
+		{
+			paths.push_back(currentPath);
+			return;
+		}
+		for(std::set<Node*>::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter)
+		{
+			(*iter)->expand(paths, currentPath);
+		}
+	}
+
+private:
+	void addChild(Node * child)
+	{
+		_children.insert(child);
+	}
+	void removeChild(Node * child)
+	{
+		_children.erase(child);
+	}
+
+private:
+	std::set<Node*> _children;
+	Node * _parent;
+	int _id;
+};
+
+}
+
+#endif /* NODE_H_ */

corelib/src/Parameters.cpp

@@ -0,0 +1,80 @@
+/*
+ * 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 "rtabmap/core/Parameters.h"
+#include <utilite/UDirectory.h>
+#include <utilite/ULogger.h>
+
+namespace rtabmap
+{
+
+Parameters * Parameters::instance_ = 0;
+UDestroyer<Parameters> Parameters::destroyer_;
+ParametersMap Parameters::parameters_;
+
+Parameters::Parameters()
+{
+}
+
+Parameters::~Parameters()
+{
+}
+
+const ParametersMap & Parameters::getDefaultParameters()
+{
+	return Parameters::getInstance()->getParameters();
+}
+
+Parameters * Parameters::getInstance()
+{
+	if(!instance_)
+	{
+		instance_ = new Parameters();
+		destroyer_.setDoomed(instance_);
+	}
+	return instance_;
+}
+
+const ParametersMap & Parameters::getParameters() const
+{
+	return parameters_;
+}
+
+void Parameters::addParameter(const std::string & key, const std::string & value)
+{
+	parameters_.insert(ParametersPair(key, value));
+}
+
+std::string Parameters::getDefaultWorkingDirectory()
+{
+	std::string path = UDirectory::homeDir();
+	if(!path.empty())
+	{
+		UDirectory::makeDir(path += "/Documents");
+		UDirectory::makeDir(path += "/RTAB-Map");
+		path += "/"; // add trailing separator
+	}
+	else
+	{
+		UFATAL("Can't get the HOME variable environment!");
+	}
+	return path;
+}
+
+}

corelib/src/RtabmapEvent.cpp

@@ -0,0 +1,150 @@
+/*
+ * 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 "rtabmap/core/RtabmapEvent.h"
+
+namespace rtabmap {
+std::map<std::string, float> Statistics::_defaultData;
+bool Statistics::_defaultDataInitialized = false;
+
+const std::map<std::string, float> & Statistics::defaultData()
+{
+	Statistics stat;
+	return _defaultData;
+}
+
+Statistics::Statistics() :
+	_extended(0),
+	_refImageId(0),
+	_loopClosureId(0),
+	_refImage(0),
+	_loopClosureImage(0)
+{
+	_defaultDataInitialized = true;
+}
+Statistics::Statistics(const Statistics & s) :
+	_extended(0),
+	_refImageId(0),
+	_loopClosureId(0),
+	_refImage(0),
+	_loopClosureImage(0)
+{
+	*this = s;
+}
+Statistics::~Statistics()
+{
+	if(_refImage)
+	{
+		cvReleaseImage(&_refImage);
+	}
+	if(_loopClosureImage)
+	{
+		cvReleaseImage(&_loopClosureImage);
+	}
+}
+
+// name format = "Grp/Name/unit"
+void Statistics::addStatistic(const std::string & name, float value)
+{
+	_data.insert(std::pair<std::string, float>(name, value));
+}
+
+//take the ownership of the image, the image will be
+//deleted in the 'Statistics' destructor
+void Statistics::setRefImage(IplImage ** refImage)
+{
+	if(_refImage)
+		cvReleaseImage(&_refImage);
+	_refImage = *refImage;
+}
+
+// Copy the image
+void Statistics::setRefImage(const IplImage * refImage)
+{
+	if(_refImage)
+		cvReleaseImage(&_refImage);
+	if(refImage)
+	{
+		_refImage = cvCloneImage(refImage);
+	}
+	else
+	{
+		_refImage = 0;
+	}
+}
+
+//take the ownership of the image, the image will be
+//deleted in the 'Statistics' destructor
+void Statistics::setLoopClosureImage(IplImage ** loopClosureImage)
+{
+	if(_loopClosureImage)
+		cvReleaseImage(&_loopClosureImage);
+	_loopClosureImage = *loopClosureImage;
+}
+
+// Copy the image
+void Statistics::setLoopClosureImage(const IplImage * loopClosureImage)
+{
+	if(_loopClosureImage)
+		cvReleaseImage(&_loopClosureImage);
+	if(loopClosureImage)
+	{
+		_loopClosureImage = cvCloneImage(loopClosureImage);
+	}
+	else
+	{
+		_loopClosureImage = 0;
+	}
+}
+
+Statistics & Statistics::operator=(const Statistics & s)
+{
+	ULOGGER_DEBUG("");
+	_data = s.data();
+	if(_refImage)
+	{
+		cvReleaseImage(&_refImage);
+		_refImage = 0;
+	}
+	if(_loopClosureImage)
+	{
+		cvReleaseImage(&_loopClosureImage);
+		_loopClosureImage = 0;
+	}
+	_extended = s.extended();
+	_refImageId = s.refImageId();
+	_loopClosureId = s.loopClosureId();
+	if(s.refImage())
+	{
+		_refImage = cvCloneImage(s.refImage());
+	}
+	if(s.loopClosureImage())
+	{
+		_loopClosureImage = cvCloneImage(s.loopClosureImage());
+	}
+	_posterior = s.posterior();
+	_likelihood = s.likelihood();
+	_weights = s.weights();
+	_refWords = s.refWords();
+	_loopWords = s.loopWords();
+
+	return *this;
+}
+
+}

corelib/src/Signature.cpp

@@ -0,0 +1,243 @@
+/*
+ * 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 "Signature.h"
+#include "Memory.h"
+#include <opencv2/highgui/highgui.hpp>
+#include "VerifyHypotheses.h"
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap
+{
+
+Signature::~Signature()
+{
+	ULOGGER_DEBUG("id=%d", _id);
+	if(_image)
+	{
+		cvReleaseImage(&_image);
+	}
+}
+
+Signature::Signature(int id, const IplImage * image, bool keepImage) :
+		_id(id),
+		_weight(0),
+		_loopClosureId(0),
+		_image(0),
+		_saved(false),
+		_width(0),
+		_height(0)
+{
+	if(image)
+	{
+		_width = image->width;
+		_height = image->height;
+		if(keepImage)
+		{
+			_image = cvCloneImage(image);
+		}
+	}
+}
+
+// Warning, the image returned must be released
+const IplImage * Signature::getImage() const
+{
+	return _image;
+}
+
+void Signature::setImage(const IplImage * image)
+{
+	if(_image && image)
+	{
+		cvReleaseImage(&_image);
+		_image = cvCloneImage(image);
+	}
+	else
+	{
+		UWARN("Parameter is null or no image is saved.");
+	}
+}
+
+// Warning, the matrix returned must be released
+CvMat * Signature::compressImage(const IplImage * image)
+{
+	if(!image)
+	{
+		UERROR("The parameter must not be null.");
+		return 0;
+	}
+	// Compress image
+	int params[3] = {0};
+
+	//JPEG compression
+	std::string format = "jpeg";
+	params[0] = CV_IMWRITE_JPEG_QUALITY;
+	params[1] = 80; // default: 80% quality
+
+	//PNG compression
+	//std::string format = "png";
+	//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+	//params[1] = 9; // default: maximum compression
+
+	std::string extension = '.' + format;
+	return cvEncodeImage(extension.c_str(), image, params);
+}
+
+// Warning, the image returned must be released
+IplImage * Signature::decompressImage(const CvMat * imageCompressed)
+{
+	if(!imageCompressed)
+	{
+		UERROR("The parameter must not be null.");
+		return 0;
+	}
+	return cvDecodeImage(imageCompressed, CV_LOAD_IMAGE_ANYCOLOR);
+}
+
+void Signature::addNeighbors(const NeighborsMap & neighbors)
+{
+	for(NeighborsMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	{
+		this->addNeighbor(i->first, i->second);
+		//UDEBUG("%d -> %d, a=%d", this->id(), i->first, i->second.size());
+	}
+}
+
+void Signature::addNeighbor(int neighbor, const std::list<std::vector<float> > & actions)
+{
+	ULOGGER_DEBUG("Adding neighbor %d to %d with %d actions", neighbor, this->id(), actions.size());
+	std::pair<NeighborsMap::iterator, bool> inserted = _neighbors.insert(std::pair<int, std::list<std::vector<float> > >(neighbor, actions));
+	//UDEBUG("%d -> %d, a=%d", this->id(), neighbor, actions.size());
+	if(!inserted.second)
+	{
+		ULOGGER_ERROR("neighbor %d already added to %d", neighbor, this->id());
+		return;
+	}
+	if(neighbor == _id)
+	{
+		ULOGGER_ERROR("same Id ? (%d)", neighbor, this->id());
+		return;
+	}
+}
+
+void Signature::removeNeighbor(int neighbor)
+{
+	ULOGGER_DEBUG("Removing neighbor %d to %d", neighbor, this->id());
+	// we delete the first found because there is not supposed
+	// to have more than one occurrence of this neighbor (see addNeighbor())
+	int erased = _neighbors.erase(neighbor);
+	if(!erased)
+	{
+		ULOGGER_WARN("neighbor %d not found in %d", neighbor, this->id());
+	}
+}
+
+
+
+
+//KeypointSignature
+KeypointSignature::KeypointSignature(
+		const std::multimap<int, cv::KeyPoint> & words,
+		int id,
+		const IplImage * image,
+		bool keepRawData) :
+	Signature(id, image, keepRawData),
+	_words(words),
+	_enabled(false)
+{
+}
+
+KeypointSignature::KeypointSignature(int id) :
+	Signature(id),
+	_enabled(false)
+{
+}
+
+KeypointSignature::~KeypointSignature()
+{
+}
+
+float KeypointSignature::compareTo(const Signature * s) const
+{
+	const KeypointSignature * ss = dynamic_cast<const KeypointSignature *>(s);
+	float similarity = 0;
+
+	if(ss) //Compatible
+	{
+		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
+
+		if(words.size() != 0 && _words.size() != 0)
+		{
+			std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
+			std::list<int> pairsId;
+			int totalWords = _words.size()>words.size()?_words.size():words.size();
+			VerifyHypothesesEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
+
+			similarity = float(pairs.size()) / float(totalWords);
+
+			// Adjust similarity with the ratio of words between the signatures
+			/*float ratio = 1;
+			if(_words.size() > words.size() && _words.size())
+			{
+				ratio = float(words.size()) / float(_words.size());
+			}
+			else
+			{
+				ratio = float(_words.size()) / float(words.size());
+			}
+
+			similarity *= ratio;*/
+		}
+	}
+	return similarity;
+}
+
+void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
+{
+	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
+	if(kps.size())
+	{
+		_words.erase(oldWordId);
+		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
+		{
+			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
+		}
+	}
+}
+
+#define BAD_SIGNATURE_THRESHOLD 0 // elements
+bool KeypointSignature::isBadSignature() const
+{
+	if(_words.size() <= BAD_SIGNATURE_THRESHOLD)
+		return true;
+	return false;
+}
+
+void KeypointSignature::removeAllWords()
+{
+	_words.clear();
+}
+
+void KeypointSignature::removeWord(int wordId)
+{
+	_words.erase(wordId);
+}
+
+}

corelib/src/Signature.h

@@ -0,0 +1,131 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <map>
+#include <list>
+#include <vector>
+
+//TODO : add copy constructor
+
+namespace rtabmap
+{
+
+class Memory;
+typedef std::map<int, std::list<std::vector<float> > > NeighborsMap;
+
+class RTABMAP_EXP Signature
+{
+public:
+	static CvMat * compressImage(const IplImage * image);
+	static IplImage * decompressImage(const CvMat * imageCompressed);
+
+public:
+	virtual ~Signature();
+
+	/**
+	 * Must return a value between >=0 and <=1 (1 means 100% similarity)
+	 */
+	virtual float compareTo(const Signature * signature) const = 0;
+	virtual bool isBadSignature() const = 0;
+	virtual std::string signatureType() const = 0;
+
+	const IplImage * getImage() const;
+	void setImage(const IplImage * image);
+
+	int id() const {return _id;}
+
+	void addNeighbors(const NeighborsMap & neighbors);
+	void addNeighbor(int neighborId, const std::list<std::vector<float> > & actions);
+	void removeNeighbor(int neighborId);
+	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
+	void setWeight(int weight) {_weight = weight;}
+	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setWidth(int width) {_width = width;}
+	void setHeight(int height) {_height = height;}
+	void setSaved(bool saved) {_saved = saved;}
+
+	const NeighborsMap & getNeighbors() const {return _neighbors;}
+	int getWeight() const {return _weight;}
+	int getLoopClosureId() const {return _loopClosureId;}
+	int getWidth() const {return _width;}
+	int getHeight() const {return _height;}
+	bool isSaved() const {return _saved;}
+
+protected:
+	Signature(int id, const IplImage * image = 0, bool keepImage = false);
+
+private:
+	int _id;
+	NeighborsMap _neighbors; // id, [action1, action2, ...] All actions must have the same length
+	int _weight;
+	int _loopClosureId;
+	IplImage * _image;
+	bool _saved; // If it's saved to bd
+	int _width; // pixels
+	int _height; // pixels
+};
+
+
+class KeypointDetector;
+class VWDictionary;
+
+class RTABMAP_EXP KeypointSignature :
+	public Signature
+{
+public:
+	KeypointSignature(
+			const std::multimap<int, cv::KeyPoint> & words,
+			int id,
+			const IplImage * image = 0,
+			bool keepRawData = false);
+	KeypointSignature(int id);
+
+	virtual ~KeypointSignature();
+
+	virtual float compareTo(const Signature * signature) const;
+	virtual bool isBadSignature() const;
+	virtual std::string signatureType() const {return "KeypointSignature";};
+
+	void removeAllWords();
+	void removeWord(int wordId);
+	void changeWordsRef(int oldWordId, int activeWordId);
+
+	void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
+	bool isEnabled() const {return _enabled;}
+	void setEnabled(bool enabled) {_enabled = enabled;}
+	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+
+private:
+	// Contains all words (Some can be duplicates -> if a word appears 2
+	// times in the signature, it will be 2 times in this list)
+	// Words match with the CvSeq keypoints and descriptors
+	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	bool _enabled;
+
+};
+
+
+} // namespace rtabmap

corelib/src/VWDictionary.h

@@ -0,0 +1,116 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "VisualWord.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class NearestNeighbor;
+class DBDriver;
+
+class RTABMAP_EXP VWDictionary
+{
+public:
+	enum NNStrategy{kNNNaive, kNNKdTree, kNNFlannKdTree, kNNUndef};
+	static const int ID_START;
+	static const int ID_INVALID;
+
+public:
+	VWDictionary(const ParametersMap & parameters = ParametersMap());
+	virtual ~VWDictionary();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	virtual void update();
+
+	virtual std::list<int> addNewWords(
+			const std::list<std::vector<float> > & descriptors,
+			unsigned int dim,
+			int signatureId);
+	virtual void addWord(VisualWord * vw);
+
+	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws, bool searchInNewlyAddedWords = true) const;
+	void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, unsigned int length, std::map<float, int> & results, unsigned int k) const;
+
+	void addWordRef(int wordId, int signatureId);
+	void removeAllWordRef(int wordId, int signatureId);
+	const VisualWord * getWord(int id) const;
+	void setWordSaved(int id, bool saved);
+	void setLastWordId(int id) {_lastWordId = id;}
+	void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
+	const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
+	void setMinDist(float d);
+	float getMinDist() const {return _minDist;}
+	bool isMinDistUsed() const {return _minDistUsed;}
+	void setMinDistUsed(bool used) {_minDistUsed = used;}
+	void setNndrUsed(bool used) {_nndrUsed = used;}
+	bool isNndrUsed() const {return _nndrUsed;}
+	void setNndrRatio(float ratio);
+	float getNndrRatio() {return _nndrRatio;}
+	unsigned int getNotIndexedWordsCount() const {return _visualWords.size() - _mapIndexId.size();}
+	unsigned int getLastNewWordsAddedCount() const {return _lastNewWordsAddedCount;}
+	int getLastIndexedWordId() const;
+	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
+	NNStrategy nnStrategy() const;
+	bool isIncremental() const {return _incrementalDictionary;}
+	void setIncrementalDictionary(bool incrementalDictionary, const std::string & dictionaryPath);
+
+	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
+
+	void clear();
+	std::vector<VisualWord *> getUnusedWords() const;
+	unsigned int getUnusedWordsSize() const {return _unusedWords.size();}
+	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
+
+protected:
+	int getNextId();
+
+protected:
+	std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
+	unsigned int _lastNewWordsAddedCount;
+	int _totalActiveReferences; // keep track of all references for updating the common signature
+
+private:
+	bool _incrementalDictionary;
+	bool _minDistUsed;
+	float _minDist; //euclidean distance ^ 2
+	bool _nndrUsed;
+	float _nndrRatio;
+	unsigned int _maxLeafs;
+	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
+	unsigned int _dim;
+	int _lastWordId;
+	NearestNeighbor * _nn;
+	cv::Mat _dataTree;
+	std::map<int ,int> _mapIndexId;
+	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>
+};
+
+} // namespace rtabmap

corelib/src/VerifyHypotheses.cpp

@@ -0,0 +1,595 @@
+/*
+ * 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 "VerifyHypotheses.h"
+#include "rtabmap/core/Parameters.h"
+#include "Signature.h"
+#include "Memory.h"
+#include <cstdlib>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap
+{
+
+VerifyHypotheses::VerifyHypotheses(const ParametersMap & parameters) :
+	_status(0)
+{
+	this->parseParameters(parameters);
+}
+
+void VerifyHypotheses::parseParameters(const ParametersMap & parameters)
+{
+}
+
+
+
+/////////////////////////
+// VerifyHypothesesSimple
+/////////////////////////
+VerifyHypothesesSimple::VerifyHypothesesSimple(const ParametersMap & parameters) :
+	VerifyHypotheses(parameters)
+{
+	this->parseParameters(parameters);
+}
+
+VerifyHypothesesSimple::~VerifyHypothesesSimple()
+{
+}
+
+void VerifyHypothesesSimple::parseParameters(const ParametersMap & parameters)
+{
+	VerifyHypotheses::parseParameters(parameters);
+}
+
+int VerifyHypothesesSimple::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+{
+	int hypothesis = 0;
+
+	if(!hypotheses.empty())
+	{
+		for(std::list<int>::const_iterator i = hypotheses.begin(); i!=hypotheses.end(); ++i)
+		{
+			if(*i > 0)
+			{
+				hypothesis = *i;
+				break;
+			}
+		}
+	}
+
+	return hypothesis;
+}
+
+
+/////////////////////////
+// VerifyHypothesesSignSeq
+/////////////////////////
+/*VerifyHypothesesSignSeq::VerifyHypothesesSignSeq(const ParametersMap & parameters) :
+	VerifyHypotheses(parameters),
+	_seqLength(Parameters::defaultVhEpSeqLength())
+{
+	this->parseParameters(parameters);
+}
+
+VerifyHypothesesSignSeq::~VerifyHypothesesSignSeq() {
+
+}
+
+void VerifyHypothesesSignSeq::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kVhEpSeqLength())) != parameters.end())
+	{
+		_seqLength = atoi((*iter).second.c_str());
+	}
+	VerifyHypotheses::parseParameters(parameters);
+}
+
+int VerifyHypothesesSignSeq::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+{
+	int hypothesis = 0;
+	std::map<int, int> hypothesesToKeep;
+
+	// update hypotheses
+	std::map<int, int>::iterator tmp;
+	for(std::list<int>::const_iterator j=hypotheses.begin(); j!=hypotheses.end(); ++j)
+	{
+		// Add it like a new hypothesis
+		hypothesesToKeep.insert(std::pair<int, int>(*j, 1)); // NOTE : It will be deleted if an updated hypothesis gives the same id.
+
+		// If we have already this hypothesis, just keep it
+		tmp = _hypotheses.find(*j);
+		if(tmp != _hypotheses.end())
+		{
+			hypothesesToKeep.insert(std::pair<int, int>((*tmp).first, (*tmp).second));
+		}
+
+		// Forward hypothesis
+		tmp = _hypotheses.find(*j-1);
+		if(tmp != _hypotheses.end())
+		{
+			hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second+1));
+		}
+
+		// Backward hypothesis
+		tmp = _hypotheses.find(*j+1);
+		if(tmp != _hypotheses.end())
+		{
+			hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second-1));
+		}
+	}
+
+	_hypotheses = hypothesesToKeep;
+
+	// if an hypothesis has at least 3 references, return the id
+	if(_hypotheses.size()>0)
+	{
+		for(std::map<int, int>::iterator i=_hypotheses.begin(); i!=_hypotheses.end(); ++i)
+		{
+			if((*i).second > _seqLength)
+			{
+				hypothesis = (*i).first;
+				break; // return the first
+			}
+		}
+	}
+
+	return hypothesis;
+}*/
+
+
+
+/////////////////////////
+// VerifyHypothesesEpipolarGeo
+/////////////////////////
+VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & parameters) :
+	VerifyHypotheses(parameters),
+	_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
+	_ransacParam1(Parameters::defaultVhEpRansacParam1()),
+	_ransacParam2(Parameters::defaultVhEpRansacParam2())
+{
+	this->parseParameters(parameters);
+}
+
+VerifyHypothesesEpipolarGeo::~VerifyHypothesesEpipolarGeo() {
+
+}
+
+void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
+	{
+		_matchCountMinAccepted = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
+	{
+		_ransacParam1 = std::atof((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
+	{
+		_ransacParam2 = std::atof((*iter).second.c_str());
+	}
+	VerifyHypotheses::parseParameters(parameters);
+}
+
+void VerifyHypothesesEpipolarGeo::setStatus(int status)
+{
+	// Only set if the status was not set before. This will keep the
+	// first error (when comparing with many signatures)
+	if(status == UNDEFINED || this->getStatus() == UNDEFINED || status == ACCEPTED)
+	{
+		VerifyHypotheses::setStatus(status);
+	}
+}
+
+int VerifyHypothesesEpipolarGeo::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+{
+	ULOGGER_DEBUG("");
+	int hypothesis = 0;
+	this->setStatus(UNDEFINED);
+
+	if(mem && !hypotheses.empty())
+	{
+		const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(mem->getLastSignature());
+		if(ssRef)
+		{
+			unsigned int i=0;
+			for(std::list<int>::const_iterator iter = hypotheses.begin(); iter!=hypotheses.end(); ++iter)
+			{
+				if(*iter > 0)
+				{
+					const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(mem->getSignature(*iter));
+					if(ssHyp)
+					{
+						if(doEpipolarGeometry(ssHyp, ssRef))
+						{
+							hypothesis = *iter;
+							break;
+						}
+					}
+				}
+				++i;
+			}
+		}
+	}
+	else if(!mem)
+	{
+		this->setStatus(this->MEMORY_IS_NULL);
+	}
+	else if(hypotheses.empty())
+	{
+		this->setStatus(this->NO_HYPOTHESIS);
+	}
+
+	return hypothesis;
+}
+
+bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
+{
+	if(ssA == 0 || ssB == 0)
+	{
+		this->setStatus(this->NULL_MATCHING_SURF_SIGNATURES);
+		return false;
+	}
+	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
+
+	std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
+	std::list<int> pairsId;
+
+	//bool allPairs = true;
+	int realPairsCount = 0;
+
+
+	realPairsCount = findPairsOne(ssA->getWords(), ssB->getWords(), pairs, pairsId);
+	ULOGGER_DEBUG("%d %d", pairs.size(), pairsId.size());
+	int pairsCount = pairs.size();
+	ULOGGER_DEBUG("id(%d,%d) realPairsCount found=%d, pairsCount=%d...", ssA->id(), ssB->id(), realPairsCount, pairsCount);
+
+	int similarities = this->getTotalSimilarities(ssA->getWords(), ssB->getWords());
+
+	ULOGGER_DEBUG("realPairsCount=%d, "
+			"test1=%f%%, "
+			"test2=%f%%, "
+			"similarities/total=%f%%, "
+			"realP/similarities=%f%%, "
+			"(pairs/2)/similarities=%f%%",
+			realPairsCount,
+			float(realPairsCount)/(float(ssA->getWords().size() + ssB->getWords().size())/2),
+			float(pairs.size())/(float(ssA->getWords().size() + ssB->getWords().size())/2),
+			float(similarities)/float(ssA->getWords().size() + ssB->getWords().size()),
+			float(realPairsCount) / float(similarities),
+			float(pairs.size()) / float(similarities));
+	if(pairsCount < _matchCountMinAccepted)
+	{
+		this->setStatus(this->NOT_ENOUGH_MATCHING_PAIRS);
+		return false;
+	}
+
+	//Convert Keypoints to a structure that OpenCV understands
+	//3 dimensions (Homogeneous vectors)
+	cv::Mat points1(1, pairs.size(), CV_32FC2);
+	cv::Mat points2(1, pairs.size(), CV_32FC2);
+
+	float * points1data = points1.ptr<float>(0);
+	float * points2data = points2.ptr<float>(0);
+
+	// Fill the points here ...
+	int i=0;
+	for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::const_iterator iter = pairs.begin();
+		iter != pairs.end();
+		++iter )
+	{
+		points1data[i*2] = (*iter).first.pt.x;
+		points1data[i*2+1] = (*iter).first.pt.y;
+
+		points2data[i*2] = (*iter).second.pt.x;
+		points2data[i*2+1] = (*iter).second.pt.y;
+
+		// the output of the correspondences can be easily copied in MatLab
+		/*if(i==0)
+		{
+			ULOGGER_DEBUG("pt x=[%f;%f;1;%d];,xp=[%f;%f;1;%d];",
+					(*iter).first.pt.x,
+					(*iter).first.pt.y,
+					Util::valueAt(pairsId,i),
+					(*iter).second.pt.x,
+					(*iter).second.pt.y,
+					Util::valueAt(pairsId,i));
+		}
+		else
+		{
+			ULOGGER_DEBUG("pt x=[x [%f;%f;1;%d]];,xp=[xp [%f;%f;1;%d]];",
+					(*iter).first.pt.x,
+					(*iter).first.pt.y,
+					Util::valueAt(pairsId,i),
+					(*iter).second.pt.x,
+					(*iter).second.pt.y,
+					Util::valueAt(pairsId,i));
+		}*/
+		++i;
+	}
+
+	UTimer timer;
+	timer.start();
+
+	// Find the fundamental matrix
+	cv::vector<uchar> status;
+	cv::Mat fundamentalMatrix = cv::findFundamentalMat(
+				points1,
+				points2,
+				status,
+				CV_FM_RANSAC,
+				_ransacParam1,
+				_ransacParam2);
+
+	ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
+
+		// Fundamental matrix is valid ?
+	bool fundMatFound = false;
+	if(fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_FATAL("fundamentalMatrix.type() != CV_64FC1");
+	}
+	if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
+	   (fundamentalMatrix.at<double>(0,0) != 0.0 ||
+	    fundamentalMatrix.at<double>(0,1) != 0.0 ||
+	    fundamentalMatrix.at<double>(0,2) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,0) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,1) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,2) != 0.0 ||
+		fundamentalMatrix.at<double>(2,0) != 0.0 ||
+		fundamentalMatrix.at<double>(2,1) != 0.0 ||
+		fundamentalMatrix.at<double>(2,2) != 0.0) )
+
+	{
+		fundMatFound = true;
+	}
+
+	ULOGGER_DEBUG("id(%d,%d) fm_count=%d...", ssA->id(), ssB->id(), fundMatFound);
+
+	if(fundMatFound)
+	{
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > inliers;
+		std::list<int> inliersId;
+
+		int goodCount = 0;
+		float total = 0;
+		std::list<std::pair<float, float> > ptsAddedA;
+		std::list<std::pair<float, float> > ptsAddedB;
+		cv::Mat x(3, 1, fundamentalMatrix.type());
+		cv::Mat xp(1, 3, fundamentalMatrix.type());
+		int i=0;
+		for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
+		{
+			//if(status[i])
+			{
+				if(uContains(ptsAddedA, std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y)))
+				{
+					ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).first.pt.x, (*iter).first.pt.y);
+				}
+				else if(uContains(ptsAddedB, std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y)))
+				{
+					ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).second.pt.x, (*iter).second.pt.y);
+				}
+				else
+				{
+					double * xData = x.ptr<double>(0);
+					double * xpData = xp.ptr<double>(0);
+					xData[0] = (*iter).first.pt.x;
+					xData[1] = (*iter).first.pt.y;
+					xData[2] = 1;
+					xpData[0] = (*iter).second.pt.x;
+					xpData[1] = (*iter).second.pt.y;
+					xpData[2] = 1;
+					cv::Mat r = xp * (fundamentalMatrix * x);
+					//if((r->data.fl[0] < 0 ? -r->data.fl[0]:r->data.fl[0]) < 1000000)
+					{
+						// Add only once a pair for the same id, used when a point matches with more than one...
+						ptsAddedA.push_back(std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y));
+						ptsAddedB.push_back(std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y));
+						if(status[i])
+						{
+							inliers.push_back(*iter);
+							inliersId.push_back(uValueAt(pairsId, i));
+							goodCount++;
+						}
+						//ULOGGER_DEBUG("[%d] status=%d, r->data.fl[0]=%f, Added!", Util::valueAt(pairsId,i), status[i], r.ptr<double>(0)[0]);
+					}
+					/*else
+					{
+						ULOGGER_DEBUG("status=%d, r->data.fl[0]=%f, Not added!", status->data.ptr[i], r->data.fl[0]);
+					}*/
+					total+=(r.ptr<double>(0)[0] < 0 ? -r.ptr<double>(0)[0]:r.ptr<double>(0)[0]);
+				}
+			}
+			/*else
+			{
+				ULOGGER_DEBUG("VHEpipolarGeo::doEpipolarGeometry() status=%d", status[i]);
+			}*/
+			++i;
+		}
+
+		ULOGGER_DEBUG("pairs/realPairs=%d/%d -> %d%%, goodCount=%d -> %d%%, good/real = %d%%, totalMean=%f",
+				pairsCount,
+				realPairsCount,
+				int(float(pairsCount)/float(realPairsCount*100)),
+				goodCount,
+				int(float(goodCount)/float(pairsCount*100)),
+				int(float(goodCount)/float(realPairsCount*100)),
+				total/float(realPairsCount));
+
+		// Show the fundamental matrix
+		ULOGGER_DEBUG(
+			"F = [%f %f %f;%f %f %f;%f %f %f]",
+			fundamentalMatrix.ptr<double>(0)[0],
+			fundamentalMatrix.ptr<double>(0)[1],
+			fundamentalMatrix.ptr<double>(0)[2],
+			fundamentalMatrix.ptr<double>(0)[3],
+			fundamentalMatrix.ptr<double>(0)[4],
+			fundamentalMatrix.ptr<double>(0)[5],
+			fundamentalMatrix.ptr<double>(0)[6],
+			fundamentalMatrix.ptr<double>(0)[7],
+			fundamentalMatrix.ptr<double>(0)[8]);
+
+		if(goodCount < _matchCountMinAccepted)
+		{
+			this->setStatus(this->EPIPOLAR_CONSTRAINT_FAILED);
+			ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d), min is %d", goodCount, _matchCountMinAccepted);
+			return false;
+		}
+		else
+		{
+			this->setStatus(this->ACCEPTED);
+			return true;
+		}
+	}
+	this->setStatus(this->FUNDAMENTAL_MATRIX_NOT_FOUND);
+	return false;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
+ * realPairsCount = 5
+ */
+int VerifyHypothesesEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
+	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
+	pairs.clear();
+	int realPairsCount = 0;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		iterA = wordsA.find(*i);
+		iterB = wordsB.find(*i);
+		while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+		{
+			pairsId.push_back(*i);
+			pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second));
+			++iterA;
+			++iterB;
+			++realPairsCount;
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
+ * realPairsCount = 5
+ */
+int VerifyHypothesesEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	int realPairsCount = 0;
+	pairs.clear();
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
+		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
+		if(ptsA.size() == 1 && ptsB.size() == 1)
+		{
+			pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front()));
+			pairsId.push_back(*i);
+			++realPairsCount;
+		}
+		else if(ptsA.size()>1 && ptsB.size()>1)
+		{
+			// just update the count
+			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
+ * realPairsCount = 5
+ */
+int VerifyHypothesesEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	UTimer timer;
+	timer.start();
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	pairs.clear();
+	int realPairsCount = 0;;
+	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
+
+		realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+		for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+		{
+			for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+			{
+				pairsId.push_back(*iter);
+				pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter));
+			}
+		}
+	}
+	ULOGGER_DEBUG("time = %f", timer.ticks());
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [1 2 4 6]
+ * return 4
+ */
+std::list<int> VerifyHypothesesEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB)
+{
+	std::list<int> sameIds;
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		if(wordsB.find(*i) != wordsB.end())
+		{
+			sameIds.push_back(*i);
+		}
+	}
+	return sameIds;
+}
+
+int VerifyHypothesesEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	int total = 0;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		total += uValues(wordsA, *i).size();
+		total += uValues(wordsB, *i).size();
+	}
+	return total;
+}
+
+}

corelib/src/VerifyHypotheses.h

@@ -0,0 +1,140 @@
+/*
+ * 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 VERIFYHYPOTHESES_H_
+#define VERIFYHYPOTHESES_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <list>
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UEventsHandler.h"
+#include <map>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
+namespace rtabmap
+{
+
+class Memory;
+
+class RTABMAP_EXP VerifyHypotheses
+{
+public:
+	virtual ~VerifyHypotheses() {}
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem) = 0;
+
+	int getStatus() {return _status;}
+	virtual void parseParameters(const ParametersMap & parameters);
+
+protected:
+	VerifyHypotheses(const ParametersMap & parameters = ParametersMap());
+	virtual void setStatus(int status) {_status = status;}
+
+private:
+	int _status;
+};
+
+
+/////////////////////////
+// VerifyHypothesesSimple
+/////////////////////////
+class VerifyHypothesesSimple : public VerifyHypotheses {
+public:
+	VerifyHypothesesSimple(const ParametersMap & parameters = ParametersMap());
+	virtual ~VerifyHypothesesSimple();
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual void parseParameters(const ParametersMap & parameters);
+};
+
+
+/////////////////////////
+// VerifyHypothesesSignSeq
+/////////////////////////
+/*class VerifyHypothesesSignSeq : public VerifyHypotheses {
+public:
+	VerifyHypothesesSignSeq(const ParametersMap & parameters = ParametersMap());
+	virtual ~VerifyHypothesesSignSeq();
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	std::map<int, int> _hypotheses;
+	int _seqLength;
+
+};*/
+
+
+
+/////////////////////////
+// VerifyHypothesesEpipolarGeo
+/////////////////////////
+class KeypointSignature;
+
+class RTABMAP_EXP VerifyHypothesesEpipolarGeo : public VerifyHypotheses
+{
+public:
+	enum STATUS
+	{
+		UNDEFINED,
+		ACCEPTED,
+		NO_HYPOTHESIS,
+		MEMORY_IS_NULL,
+		NOT_ENOUGH_MATCHING_PAIRS,
+		EPIPOLAR_CONSTRAINT_FAILED,
+		NULL_MATCHING_SURF_SIGNATURES,
+		FUNDAMENTAL_MATRIX_NOT_FOUND
+	};
+
+public:
+	static int findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
+	static int findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
+	static int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
+	static std::list<int> findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
+public:
+	VerifyHypothesesEpipolarGeo(const ParametersMap & parameters = ParametersMap());
+	virtual ~VerifyHypothesesEpipolarGeo();
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	int getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
+
+	int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
+	double getRansacParam1() const {return _ransacParam1;}
+	double getRansacParam2() const {return _ransacParam2;}
+
+	void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
+	void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
+	void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
+
+protected:
+	virtual void setStatus(int status);
+
+private:
+	bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);
+
+private:
+	int _matchCountMinAccepted;
+	double _ransacParam1;
+	double _ransacParam2;
+};
+
+} // namespace rtabmap
+
+#endif /* VERIFYHYPOTHESES_H_ */

corelib/src/VisualWord.cpp

@@ -0,0 +1,78 @@
+/*
+ * 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 "VisualWord.h"
+#include "utilite/ULogger.h"
+#include "utilite/UStl.h"
+
+namespace rtabmap
+{
+
+VisualWord::VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId) :
+	_id(id),
+	_saved(false),
+	_totalReferences(0)
+{
+	_descriptor = new float[dim];
+	if(_descriptor && descriptor)
+	{
+		memcpy(_descriptor, descriptor, dim*sizeof(float));
+	}
+	else
+	{
+		ULOGGER_ERROR("not enough memory to create the descriptor...");
+	}
+	_dim = dim;
+
+	if(signatureId)
+	{
+		addRef(signatureId);
+	}
+}
+
+VisualWord::~VisualWord()
+{
+	if(_descriptor)
+	{
+		delete [] _descriptor;
+	}
+}
+
+void VisualWord::addRef(int signatureId)
+{
+	std::map<int, int>::iterator iter = _references.find(signatureId);
+	if(iter != _references.end())
+	{
+		(*iter).second += 1;
+	}
+	else
+	{
+		_references.insert(std::pair<int, int>(signatureId, 1));
+	}
+	++_totalReferences;
+}
+
+int VisualWord::removeAllRef(int signatureId)
+{
+	int removed = uTake(_references, signatureId, 0);
+	_totalReferences -= removed;
+	return removed;
+}
+
+} // namespace rtabmap

corelib/src/VisualWord.h

@@ -0,0 +1,60 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class SignatureSurf;
+
+class RTABMAP_EXP VisualWord
+{
+public:
+	VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId = 0);
+	~VisualWord();
+
+	void addRef(int signatureId);
+	int removeAllRef(int signatureId);
+
+	int getTotalReferences() const {return _totalReferences;}
+	int id() const {return _id;}
+	const float * getDescriptor() const {return _descriptor;}
+	unsigned int getDim() const {return _dim;}
+	const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
+
+	bool isSaved() const {return _saved;}
+	void setSaved(bool saved) {_saved = saved;}
+
+private:
+	int _id;
+	float * _descriptor;
+	unsigned int _dim;
+	bool _saved; // If it's saved to bd
+
+	int _totalReferences;
+	std::map<int, int> _references; // (signature id , occurrence in the signature)
+	std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
+};
+
+} // namespace rtabmap

corelib/src/resources/DatabaseSchema.sql

@@ -0,0 +1,198 @@
+-- *******************************************************************
+--  construct_avpd_db: Script for creating the database
+--   Usage:
+--       $ sqlite3 AvpdDatabase.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Signature;
+DROP TABLE SignatureType;
+DROP TABLE Neighbor;
+DROP TABLE VisualWord;
+DROP TABLE Map_SS_VW;
+DROP TABLE StatisticsAfterRun;
+DROP TABLE StatisticsAfterRunSurf;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Signature (
+	id INTEGER NOT NULL,
+	type VARCHAR NOT NULL,
+	weight INTEGER,
+	loopClosureId INTEGER,
+	image BLOB,
+	imgWidth INTEGER,
+	imgHeight INTEGER,
+	timeEnter DATE,
+	PRIMARY KEY (id),
+	FOREIGN KEY (type) REFERENCES SignatureType(type),
+	FOREIGN KEY (loopClosureId) REFERENCES Signature(id)
+);
+
+CREATE TABLE Neighbor (
+    sid INTEGER NOT NULL,
+    nid INTEGER NOT NULL,
+    actionSize INTEGER,
+    actions BLOB,
+    timeEnter DATE,
+    PRIMARY KEY (sid, nid),
+    FOREIGN KEY (sid) REFERENCES Signature(id),
+    FOREIGN KEY (nid) REFERENCES Signature(id)
+);
+
+CREATE TABLE SignatureType (
+	type VARCHAR NOT NULL,
+	PRIMARY KEY (type)
+);
+
+CREATE TABLE VisualWord (
+	id INTEGER NOT NULL,
+	descriptorSize INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	timeEnter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Map_SS_VW (
+	signatureId INTEGER NOT NULL,
+	visualWordId INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	laplacian INTEGER NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	hessian FLOAT NOT NULL,
+	timeEnter DATE,
+	FOREIGN KEY (signatureId) REFERENCES Signature(id),
+	FOREIGN KEY (visualWordId) REFERENCES VisualWord(id)
+);
+
+CREATE TABLE StatisticsAfterRun (
+	stMemSize INTEGER,
+	lastSignAdded INTEGER,
+	processMemUsed INTEGER,
+	databaseMemUsed INTEGER,
+	timeEnter DATE
+);
+
+CREATE TABLE StatisticsAfterRunSurf (
+	dictionarySize INTEGER,
+	timeEnter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Signature BEFORE INSERT ON Signature 
+WHEN NOT EXISTS (SELECT type FROM SignatureType WHERE SignatureType.type = NEW.type)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+CREATE TRIGGER insert_Neighbor BEFORE INSERT ON Neighbor 
+WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.sid)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+CREATE TRIGGER insert_Map_SS_VW BEFORE INSERT ON Map_SS_VW 
+WHEN NOT EXISTS (SELECT type FROM Signature WHERE Signature.id = NEW.signatureId AND type='surf')
+--OR NOT EXISTS (SELECT id FROM VisualWord WHERE VisualWord.id = NEW.visualWordId)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+ --   Creating a trigger for timeEnter
+CREATE TRIGGER insert_Signature_timeEnter AFTER INSERT ON Signature
+BEGIN
+ UPDATE Signature SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Neighbor_timeEnter AFTER INSERT ON Neighbor
+BEGIN
+ UPDATE Neighbor SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_VisualWord_timeEnter AFTER INSERT ON VisualWord
+BEGIN
+ UPDATE VisualWord SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Map_SS_VW_timeEnter AFTER INSERT ON Map_SS_VW
+BEGIN
+ UPDATE Map_SS_VW SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_StatisticsAfterRun_timeEnter AFTER INSERT ON StatisticsAfterRun
+BEGIN
+ UPDATE StatisticsAfterRun SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_StatisticsAfterRunSurf_timeEnter AFTER INSERT ON StatisticsAfterRunSurf
+BEGIN
+ UPDATE StatisticsAfterRunSurf SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE INDEX IDX_Map_SS_VW_SignatureId on Map_SS_VW (signatureId);
+CREATE INDEX IDX_Map_SS_VW_VisualWordId on Map_SS_VW (visualWordId);
+CREATE INDEX IDX_Signature_Id on Signature (id);
+CREATE INDEX IDX_VisualWord_Id on VisualWord (id);
+CREATE INDEX IDX_Signature_TimeEnter on Signature (timeEnter);
+CREATE INDEX IDX_VisualWord_TimeEnter on VisualWord (timeEnter);
+CREATE INDEX IDX_Neighbor_Sid on Neighbor (sid);
+
+-- *******************************************************************
+-- Data
+-- *******************************************************************
+INSERT INTO SignatureType(type) VALUES ('fourier');
+INSERT INTO SignatureType(type) VALUES ('surf');
+
+-- *******************************************************************
+-- TESTS
+-- *******************************************************************
+-- *** Data Test ***
+/*
+INSERT INTO Signature VALUES(1, 'surf', null, null, null);
+INSERT INTO Signature VALUES(2, 'surf', null, null, null);
+INSERT INTO Signature VALUES(3, 'surf', null, null, null);
+INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
+INSERT INTO VisualWord VALUES (2, 1, 2,'0.213213 0.4352323', null);
+INSERT INTO VisualWord VALUES (3, 3, 2,'0.213213 0.4352323', null);
+INSERT INTO Map_SS_VW VALUES (1, 1, 0,0,0,0,0, null);
+INSERT INTO Map_SS_VW VALUES (2, 1, 0,0,0,0,0, null);
+INSERT INTO Map_SS_VW VALUES (2, 2, 0,0,0,0,0, null);
+*/
+
+/*
+-- For loading words
+SELECT vw.id, vw.laplacian, vw.descriptorSize, vw.descriptor, m.signatureId FROM VisualWord as vw INNER JOIN Map_SS_VW as m on vw.id=m.visualWordId ORDER BY vw.id;
+*/
+
+
+-- Refreshing the dictionary
+/*SELECT * FROM Map_SS_VW;
+SELECT * FROM VisualWord;*/
+/*
+DELETE FROM VisualWord;
+INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
+DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);
+*/
+/*SELECT * FROM Map_SS_VW;
+SELECT * FROM VisualWord;*/
+
+/*
+-- Loading only signatures on the last short time memory based on DATE
+INSERT INTO Signature VALUES(4, 'surf', null, null, null);
+INSERT INTO Signature VALUES(5, 'surf', 4, null, null);
+INSERT INTO Signature VALUES(6, 'surf', null, null, null);
+INSERT INTO Map_SS_VW VALUES (4, 1, 0,0,0,0,0, null);
+SELECT s.id FROM Signature AS s WHERE s.timeEnter >= (SELECT vw.timeEnter FROM VisualWord AS vw LIMIT 1) AND s.loopClosureId IS NULL;
+*/

corelib/tests/CMakeLists.txt

@@ -0,0 +1,37 @@
+
+SET(SRC_FILES
+    main.cpp
+    Tests.cpp
+)
+
+SET(INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../src
+    ${CPPUNIT_INCLUDE_DIR}
+    ${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+	${SQLITE3_INCLUDE_DIR}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${OpenCV_LIBRARIES} 
+	${CPPUNIT_LIBRARY}
+	${SQLITE3_LIBRARY}
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_DEFINITIONS(${CPPUNIT_DEFINITIONS})
+
+# Add binary called "testAvpdCore" that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_EXECUTABLE(testCoreLib ${SRC_FILES})
+TARGET_LINK_LIBRARIES(testCoreLib corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( testCoreLib
+PROPERTIES
+ OUTPUT_NAME testCoreLib)
+

corelib/tests/Tests.cpp

@@ -0,0 +1,483 @@
+/*
+ * 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 <cppunit/config/SourcePrefix.h>
+#include "Tests.h"
+
+//Headers for the test BEGIN
+#include "rtabmap/core/Camera.h"
+#include "Signature.h"
+#include "VWDictionary.h"
+#include "rtabmap/core/EpipolarGeometry.h"
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/SMState.h"
+#include "VerifyHypotheses.h"
+#include "rtabmap/core/Parameters.h"
+#include "BayesFilter.h"
+#include "KeypointMemory.h"
+
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/CameraEvent.h"
+
+#include "rtabmap/core/DBDriverFactory.h"
+#include "rtabmap/core/DBDriver.h"
+
+//Util lib
+#include "utilite/UtiLite.h"
+
+//Database
+#include <sqlite3.h>
+
+#include <string.h>
+
+//Headers for the test END
+
+CPPUNIT_TEST_SUITE_REGISTRATION( Tests );
+
+using namespace rtabmap;
+
+void Tests::testAvpd()
+{
+	printf("\n");
+	UTimer timer;
+	timer.start();
+	//Logger::setType(Logger::kTypeConsole);
+	//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testSurfStrategy.txt", false);
+	//Logger::setLevel(Logger::kDebug);
+	//Logger::setType(Logger::kTypeInvalid);
+
+	std::string path = "./data/090206-3";
+
+	CameraImages camera(path);
+	CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());
+
+	/* Create tasks */
+	Rtabmap ctabmap;
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kRtabmapSMStateBufferSize(), "0"));
+	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "true"));
+	parameters.insert(ParametersPair(Parameters::kMemRawDataKept(), "true"));
+	parameters.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "true"));
+	parameters.insert(ParametersPair(Parameters::kRtabmapTimeThr(), "0"));
+	parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "500"));
+	ctabmap.setWorkingDirectory("./LogTestAvpdCore/");
+	ctabmap.init(parameters);
+
+
+	/* Start thread's task */
+	IplImage * image = 0;
+
+	image = camera.takeImage();
+	int imgCount = 0;
+	while(image)
+	{
+		++imgCount;
+		printf("Processing image %d/84...\n", imgCount);
+		ctabmap.process(new SMState(image));
+		image = camera.takeImage();
+	}
+	if(image)
+	{
+		cvReleaseImage(&image);
+	}
+
+	CPPUNIT_ASSERT(imgCount == 84);
+
+	ctabmap.dumpData();
+
+	ULogger::write("testSurfStrategy end...");
+	//ULOGGER_INFO("\nTime = %fs", timer.ticks());
+
+
+
+	//adjustLikelihood()
+	std::map<int, float> likelihood;
+	likelihood.clear();
+	likelihood.insert(std::pair<int, float>(-1, 0));
+	likelihood.insert(std::pair<int, float>(1, 0));
+	likelihood.insert(std::pair<int, float>(2, 0));
+	likelihood.insert(std::pair<int, float>(3, 0));
+	ctabmap.adjustLikelihood(likelihood);
+	CPPUNIT_ASSERT(likelihood.size() == 4);
+	std::vector<float> values = uValues(likelihood);
+	for(unsigned int i=0; i<values.size(); ++i)
+	{
+		CPPUNIT_ASSERT(values[i] == 1.0);
+	}
+	likelihood.clear();
+	likelihood.insert(std::pair<int, float>(-1, 0.3));
+	likelihood.insert(std::pair<int, float>(1, 0.4));
+	likelihood.insert(std::pair<int, float>(2, 0.2));
+	likelihood.insert(std::pair<int, float>(3, 0.9));
+	ctabmap.adjustLikelihood(likelihood);
+	CPPUNIT_ASSERT(likelihood.size() == 4);
+	values = uValues(likelihood);
+	// Result wanted generated by the TestAdjustLikelihood.m script (MatLab/Tests)
+	int resultWanted2[4] = {1000,1000,1000,1309};
+	for(unsigned int i=0; i<values.size(); ++i)
+	{
+		//ULOGGER_DEBUG("%d vs %d", (int)(values[i]*1000), resultWanted2[i]);
+		CPPUNIT_ASSERT(int(values[i]*1000) == resultWanted2[i]);
+	}
+}
+
+void Tests::testCamera()
+{
+	//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testCamera.txt", false);
+	std::string path;
+	IplImage * image = 0;
+	int count;
+
+	//CameraVideo class FIXME add a video in svn and reactivate this test
+	/*path = "data/std_cam.avi";
+	CameraVideo cameraVideo(path, false, 1);
+	CPPUNIT_ASSERT( cameraVideo.init() );
+	CPPUNIT_ASSERT( cameraVideo.isIdle() == true);
+	image = cameraVideo.takeImage();
+	count = 0;
+	while(image)
+	{
+		cvReleaseImage(&image);
+		image = 0;
+		++count;
+		if(count == 10)
+		{
+			break;
+		}
+		image = cameraVideo.takeImage();
+	}
+	if(image)
+		cvReleaseImage(&image);
+	CPPUNIT_ASSERT( count == 10 );*/
+
+	//CameraImages class
+	path = "data/090206-3";
+	CameraImages cameraImages(path, false, 0, false, 80);
+	CPPUNIT_ASSERT( cameraImages.init() );
+	CPPUNIT_ASSERT( cameraImages.isIdle() == true);
+	image = cameraImages.takeImage();
+	count = 0;
+	while(image)
+	{
+		cvReleaseImage(&image);
+		++count;
+		image = cameraImages.takeImage();
+	}
+	CPPUNIT_ASSERT( count == 5 );
+
+	//CameraDatabase class
+	path = "./data/090206-3.db";
+	CameraDatabase cameraDatabase(path, false); // ignoreChildren=false;
+	CPPUNIT_ASSERT( cameraDatabase.init() );
+	CPPUNIT_ASSERT( cameraDatabase.isIdle() == true);
+	image = cameraDatabase.takeImage();
+	count = 0;
+	while(image)
+	{
+		++count;
+		cvReleaseImage(&image);
+		image = cameraDatabase.takeImage();
+	}
+	//ULOGGER_INFO("%d", count);
+	CPPUNIT_ASSERT( count == 82 );
+}
+
+void Tests::testDBDriverFactory()
+{
+	ULogger::setType(ULogger::kTypeFile, "LogTestAvpdCore/testSqlite3Database.txt", false);
+	DBDriver * dbDriver = 0;
+
+	dbDriver = DBDriverFactory::createDBDriver("sqlite3");
+	CPPUNIT_ASSERT( dbDriver );
+	delete dbDriver;
+
+	dbDriver = DBDriverFactory::createDBDriver("unknownDriver");
+	CPPUNIT_ASSERT( dbDriver == 0 );
+}
+
+// TODO not finished
+void Tests::testSqlite3Database()
+{
+	//Util::Logger::setLevel(Logger::kDebug);
+	//Util::Logger::setType(Logger::kTypeConsole);
+	ULogger::setType(ULogger::kTypeFile, "LogTestAvpdCore/testSqlite3Database.txt", false);
+
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+	DBDriver * driver = DBDriverFactory::createDBDriver("sqlite3", parameters);
+
+	CPPUNIT_ASSERT(driver);
+
+	driver->openConnection("LogTestAvpdCore/tmpDatabase.db");
+
+	delete driver;
+
+	ULogger::write("testSqlite3Database end...");
+}
+
+// TODO add some tests to test when a signature is forgotten or reactivated
+void Tests::testBayesFilter()
+{
+	//Util::Logger::setLevel(Logger::kDebug);
+	//Util::Logger::setType(Logger::kTypeConsole);
+
+	BayesFilter bayes;
+
+	//Parameters checks
+	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == Parameters::defaultBayesVirtualPlacePriorThr() );
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare(Parameters::defaultBayesPredictionLC()) == 0 );
+
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kBayesVirtualPlacePriorThr(), "0.01"));
+	parameters.insert(ParametersPair(Parameters::kBayesPredictionLC(), "0.01 0.02 0.03 0.04"));
+	bayes.parseParameters(parameters);
+	CPPUNIT_ASSERT( uNumber2str(bayes.getVirtualPlacePrior()).compare("0.01") == 0 );
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
+	bayes.setVirtualPlacePrior(-1);
+	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 0 );
+	bayes.setVirtualPlacePrior(1.1);
+	CPPUNIT_ASSERT( bayes.getVirtualPlacePrior() == 1 );
+	bayes.setVirtualPlacePrior(0.6);
+	CPPUNIT_ASSERT( uNumber2str(bayes.getVirtualPlacePrior()).compare("0.6") == 0 );
+	bayes.setPredictionLC("");
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
+	bayes.setPredictionLC("0,01 0,02");
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03 0.04") == 0 );
+	bayes.setPredictionLC("0.01 0.02");
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02") == 0 );
+	bayes.setPredictionLC("0.01 0.02 0.03");
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02") == 0 );
+
+	//reset parameters...
+	bayes = BayesFilter();
+
+	//computePosterior()
+	// Load memory with some data...
+	KeypointMemory mem;
+
+	//parameters
+	mem.setCommonSignatureUsed(true);
+	mem.setMaxStMemSize(1);
+	bayes.setPredictionLC("0 0.22 0.19 0.25 0.04 0.1 0.02 0.04 0.01 0.01");
+	bayes.setVirtualPlacePrior(0.9);
+
+	std::map<int, float> likelihood;
+	std::map<int, float> posterior;
+	float sum;
+	likelihood.insert(std::pair<int, float>(-1, 1));
+	int result[100] = {0};
+	int ri = 0;
+
+	for(int i=1; i<11; ++i)
+	{
+		//ULOGGER_DEBUG("--- %d ---", i);
+		std::list<std::pair<std::string, float> > memStats;
+		mem.update(0, memStats);
+		posterior = bayes.computePosterior(&mem, likelihood);
+		likelihood.insert(std::pair<int, float>(i, 1));
+		sum = uSum(uValues(posterior));
+		CPPUNIT_ASSERT(sum > 1.0-0.0001 && sum < 1.0+0.0001);
+		for(std::map<int, float>::const_iterator iter=posterior.begin(); iter!= posterior.end(); ++iter)
+		{
+			ULOGGER_DEBUG("%f", (*iter).second);
+			result[ri++] = int((*iter).second*1000);
+		}
+		while((ri) % 10 != 0)
+		{
+			result[ri++] = 0;
+		}
+	}
+	// Result wanted generated by the TestBayesFilter.m script (MatLab/Tests)
+	int resultWanted1[100] = {1000,0,0,0,0,0,0,0,0,0,900,99,0,0,0,0,0,0,0,0,810,113,75,0,0,0,0,0,0,0,729,109,96,64,0,0,0,0,0,0,656,100,98,87,56,0,0,0,0,0,590,93,95,93,78,49,0,0,0,0,531,86,90,92,85,69,42,0,0,0,478,80,86,90,86,77,62,37,0,0,430,75,81,87,86,80,70,55,33,0,387,69,77,83,84,80,73,63,49,29};
+	for(int i=0; i<100; ++i)
+	{
+		ULOGGER_DEBUG("%d vs %d", result[i], resultWanted1[i]);
+		CPPUNIT_ASSERT(result[i] >= resultWanted1[i]-1 && result[i] <= resultWanted1[i]+1);
+	}
+}
+
+void Tests::testKeypointMemory()
+{
+	//Util::Logger::setLevel(Logger::kInfo);
+	//Util::Logger::setType(Logger::kTypeConsole);
+
+	KeypointMemory mem;
+	std::map<int, float> likelihood;
+
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // It will be like read-only (database won't be changed in SVN)
+	parameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "true"));
+	mem.setCommonSignatureUsed(true);
+	mem.setMaxStMemSize(1);
+	mem.init("sqlite3", "./data/090206-3.db", false, parameters);
+	//ULOGGER_INFO("mem.getStMemIds().size() = %d, mem.getStMemIds().size()=%d", mem.getWorkingMemIds().size(), mem.getStMemIds().size());
+	CPPUNIT_ASSERT( mem.getWorkingMem().size() + mem.getStMem().size() == 83 );
+
+	//updateCommonSignature()
+	const KeypointSignature * virtualPlace = dynamic_cast<const KeypointSignature *>(mem.getSignature(Memory::kIdVirtual));
+	CPPUNIT_ASSERT(virtualPlace != 0);
+	std::list<int> wordIds = uKeys(virtualPlace->getWords());
+	//ULOGGER_INFO("wordIds.size()=%d", wordIds.size());
+	// Result wanted generated by the TestUpdateCommonWords.m script (MatLab/Tests)
+	int commonWordsWanted[163] = {5,8,9,21,22,23,27,30,32,34,36,40,45,53,61,62,64,67,68,77,85,86,97,98,99,100,103,106,122,124,125,127,129,131,143,157,161,164,168,169,172,175,181,182,187,196,197,208,210,217,218,219,220,234,235,237,244,252,286,307,308,310,315,327,328,346,347,350,355,362,372,373,387,393,394,398,404,412,423,428,429,441,442,456,465,470,476,495,496,498,506,520,558,572,584,585,587,596,620,634,637,643,646,647,658,667,668,681,709,721,726,741,766,777,785,790,791,793,808,809,880,896,906,908,919,933,940,954,958,974,981,1002,1023,1026,1058,1060,1071,1074,1097,1146,1152,1166,1215,1217,1229,1334,1341,1387,1478,1510,1539,1549,1566,1601,1624,1649,1693,1814,2046,2141,2424,2442,2674};
+	int j=0;
+	CPPUNIT_ASSERT(wordIds.size() == 163);
+	for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
+	{
+		//ULOGGER_INFO("%d vs %d", *i, commonWordsWanted[j]);
+		CPPUNIT_ASSERT(*i == commonWordsWanted[j]);
+		++j;
+	}
+
+	//computeLikelihood()
+	const KeypointSignature * lastSign = dynamic_cast<const KeypointSignature *>(mem.getLastSignature());
+	CPPUNIT_ASSERT(lastSign != 0);
+	wordIds = uKeys(lastSign->getWords());
+	j=0;
+	int signWordsRequired[136] = {9,24,27,37,39,40,45,46,64,64,67,67,68,68,69,69,79,79,81,85,87,95,109,113,115,117,122,124,127,135,135,139,142,143,145,157,159,161,168,181,188,188,191,192,196,197,208,208,211,213,219,239,241,247,256,257,258,307,310,343,558,587,643,760,896,960,1019,1023,1074,1197,1207,1292,1401,1403,1587,2151,2419,2571,2623,2654,2664,2674,2674,2674,2777,2780,2796,2808,2835,2844,2844,2851,2855,2862,2900,3036,3143,3238,3238,3315,3531,3531,3766,3803,4296,4405,4462,4502,4506,4509,4509,4516,4523,4533,4533,4534,4535,4536,4537,4538,4539,4540,4541,4542,4543,4544,4545,4546,4547,4548,4549,4550,4551,4552,4553,4554};
+	CPPUNIT_ASSERT(wordIds.size() == 136);
+	for(std::list<int>::iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
+	{
+		//ULOGGER_INFO("%d vs %d", *i, signWordsRequired[j]);
+		CPPUNIT_ASSERT(*i == signWordsRequired[j]);
+		++j;
+	}
+	likelihood = mem.computeLikelihood(lastSign);
+	//ULOGGER_INFO("likelihood.size() = %d", likelihood.size());
+	std::vector<float> values = uValues(likelihood);
+	int likelihoodWanted[82] = {109,157,263,203,87,66,78,49,60,40,47,43,43,55,102,102,147,0,38,61,64,74,69,103,39,20,44,33,14,14,20,12,18,8,59,19,41,26,45,117,124,173,223,74,0,10,17,53,33,24,33,43,52,68,119,124,146,159,28,68,59,115,71,95,37,18,16,49,9,28,20,9,15,11,10,35,45,73,18,92,167,219};
+	CPPUNIT_ASSERT(values.size() == 82);
+	for(unsigned int i=0; i<values.size(); ++i)
+	{
+		ULOGGER_INFO("%d vs %d", int(values[i]*1000), likelihoodWanted[i]);
+		CPPUNIT_ASSERT(int(values[i]*1000) == likelihoodWanted[i]);
+	}
+}
+
+void Tests::testVWDictionary()
+{
+	VWDictionary dictionary;
+	dictionary.setNndrUsed(false);
+	std::list<cv::KeyPoint> keypoints;
+	std::list<std::vector<float> > descriptors;
+	unsigned int dim = 2;
+	std::vector<float> v(dim);
+
+	keypoints.push_back(cv::KeyPoint(cv::Point2f(1,1), 10, 30, 500, 2, 0));
+	v[0] = 3;
+	v[1] = 4;
+	descriptors.push_back(v);
+
+	dictionary.addNewWords(descriptors, dim, 1);
+	CPPUNIT_ASSERT(dictionary.getVisualWords().size() == 1);
+
+	//Create a word with the next descriptor (the distance^2 with the first word added = 2)
+	v[0] = 4;
+	v[1] = 3;
+	VisualWord word(2, v.data(), dim, 0);
+	std::list<VisualWord*> words;
+	words.push_back(&word);
+	std::vector<int> ids;
+
+	//Naive
+	dictionary.setNNStrategy(VWDictionary::kNNNaive);
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(2.01f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 1);
+
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(1.99f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 0);
+
+
+	//Opencv kdtree
+	dictionary.setNNStrategy(VWDictionary::kNNKdTree);
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(2.01f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 1);
+
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(1.99f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 0);
+
+
+	//FLANN kdtree
+	dictionary.setNNStrategy(VWDictionary::kNNFlannKdTree);
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(2.01f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 1);
+
+	dictionary.setMinDistUsed(true);
+	dictionary.setNndrUsed(false);
+	dictionary.setMinDist(1.99f);
+	ids = dictionary.findNN(words);
+	CPPUNIT_ASSERT(ids.size() == 1);
+	CPPUNIT_ASSERT(ids.front() == 0);
+}
+
+void Tests::testVerifyHypotheses()
+{
+	std::multimap<int, cv::KeyPoint> wordsA;
+	std::multimap<int, cv::KeyPoint> wordsB;
+	std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
+	std::list<int> pairsId;
+
+	wordsA.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
+	wordsA.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
+	wordsA.insert(std::pair<int, cv::KeyPoint>(3, cv::KeyPoint(3, 3, 1)));
+	wordsA.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(4, 4, 1)));
+	wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
+	wordsA.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
+
+	wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(0, 0, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(1, cv::KeyPoint(1, 1, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(2, cv::KeyPoint(2, 2, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(4, cv::KeyPoint(3, 3, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(5, cv::KeyPoint(4, 4, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
+	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
+
+	int total = VerifyHypothesesEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
+
+	//printf("[%d,%d]\n", total, (int)pairs.size());
+	CPPUNIT_ASSERT(total == 5);
+	CPPUNIT_ASSERT(pairs.size() == 8 && pairsId.size() == 8);
+
+	std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator pairsIter=pairs.begin();
+	std::list<int>::iterator idIter = pairsId.begin();
+	for(;pairsIter!=pairs.end() && idIter != pairsId.end(); ++pairsIter, ++idIter)
+	{
+		//printf("(%d)[%f,%f] [%f,%f]\n", *idIter, pairsIter->first.pt.x, pairsIter->first.pt.y, pairsIter->second.pt.x, pairsIter->second.pt.x);
+	}
+}

corelib/tests/Tests.h

@@ -0,0 +1,77 @@
+/*
+ * 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 TESTS_H
+#define TESTS_H
+
+#include <cppunit/TestFixture.h>
+#include <cppunit/TestCaller.h>
+
+#include <cppunit/extensions/HelperMacros.h>
+
+#include "utilite/UDirectory.h"
+#include "utilite/ULogger.h"
+
+class Tests : public CppUnit::TestFixture  {
+
+  CPPUNIT_TEST_SUITE( Tests );
+  CPPUNIT_TEST( testAvpd );
+  CPPUNIT_TEST( testDBDriverFactory );
+  CPPUNIT_TEST( testSqlite3Database );
+  CPPUNIT_TEST( testBayesFilter );
+  CPPUNIT_TEST( testKeypointMemory );
+  CPPUNIT_TEST( testCamera );
+  CPPUNIT_TEST( testVWDictionary );
+  CPPUNIT_TEST( testVerifyHypotheses );
+  CPPUNIT_TEST_SUITE_END();
+
+private:
+public:
+  void setUp()
+  {
+	  if(!UDirectory::exists("./LogTestLibCore"))
+	  {
+		  UDirectory::makeDir("./LogTestLibCore");
+	  }
+
+	  ULogger::reset();
+	  ULogger::setType(ULogger::kTypeConsole);
+	  ULogger::setLevel(ULogger::kError);
+	  //Util::Logger::setLevel(Util::Logger::kDebug);
+  }
+
+  void tearDown() 
+  {
+	  ULogger::reset();
+	  ULogger::setType(ULogger::kTypeConsole);
+	  ULogger::setLevel(ULogger::kError);
+	  //Util::Logger::setLevel(Util::Logger::kDebug);
+  }
+
+  void testAvpd();
+  void testCamera();
+  void testDBDriverFactory();
+  void testSqlite3Database();
+  void testBayesFilter();
+  void testKeypointMemory();
+  void testVWDictionary();
+  void testVerifyHypotheses();
+};
+
+#endif

corelib/tests/main.cpp

@@ -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/>.
+ */
+
+#include <cppunit/BriefTestProgressListener.h>
+#include <cppunit/CompilerOutputter.h>
+#include <cppunit/extensions/TestFactoryRegistry.h>
+#include <cppunit/TestResult.h>
+#include <cppunit/TestResultCollector.h>
+#include <cppunit/TestRunner.h>
+
+#include <iostream>
+#include <fstream>
+
+#ifdef _MSC_VER
+// Use Visual C++'s memory checking functionality
+#define _CRTDBG_MAP_ALLOC
+#include <crtdbg.h>
+#endif // _MSC_VER
+
+int main( int argc, char **argv)
+{    
+    #ifdef _MSC_VER
+    //_crtBreakAlloc = 189;
+    _CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF ); 
+    #endif // _MSC_VER
+
+    //Header of the test
+    char header[150] = "-------------------------------------\n"
+                       "corelib Test\n"
+                       "-------------------------------------\n";
+    CPPUNIT_NS::stdCOut() << header;
+
+    // Create the event manager and test controller
+    CPPUNIT_NS::TestResult controller;
+
+    // Add a listener that colllects test result
+    CPPUNIT_NS::TestResultCollector result;
+    controller.addListener(&result);
+
+    // Add a listener that print dots as test run.
+    CPPUNIT_NS::BriefTestProgressListener progress;
+    controller.addListener(&progress);      
+
+    // Add the top suite to the test runner
+    CPPUNIT_NS::TestRunner runner;
+    runner.addTest(CPPUNIT_NS::TestFactoryRegistry::getRegistry().makeTest());
+    runner.run(controller);
+
+    // Print test in a compiler compatible format.
+    CPPUNIT_NS::CompilerOutputter outputter( &result, CPPUNIT_NS::stdCOut() );
+    outputter.write(); 
+
+    // If a path is passed in argument copy the result in it
+    if (argc == 2)
+    {
+        std::ofstream myfile;
+        myfile.open (argv[1]);
+        CPPUNIT_NS::CompilerOutputter outputterFile( &result, myfile);
+        myfile << header;
+        outputterFile.write(); 
+        myfile.close();
+    }
+
+    return 0;
+}