MERGE branch STM 325:449 into trunk

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

corelib/CMakeLists.txt

@@ -2,6 +2,9 @@ ADD_SUBDIRECTORY( src )
 ADD_SUBDIRECTORY( ConsoleApp )
 ADD_SUBDIRECTORY( ImagesJoiner )
 ADD_SUBDIRECTORY( WebcamCapture )
+ADD_SUBDIRECTORY( LogPolar )
+ADD_SUBDIRECTORY( ImagesDbExtractor )
+ADD_SUBDIRECTORY( ColorIndexesGenerator )
 
 IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
 	ADD_SUBDIRECTORY( DatabaseViewer )

corelib/ColorIndexesGenerator/CMakeLists.txt

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

corelib/ColorIndexesGenerator/main.cpp

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

corelib/ConsoleApp/CMakeLists.txt

@@ -5,13 +5,13 @@ SET(SRC_FILES
 
 SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBRARIES} 
 )
 

corelib/ConsoleApp/main.cpp

@@ -33,25 +33,26 @@
 
 using namespace rtabmap;
 
-#define GENERATED_GT_NAME "GroundTruth_generated.txt"
+#define GENERATED_GT_NAME "GroundTruth_generated.bmp"
 
 void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-console [options] \"path\"\n"
-			"  path                            For images, use the directory path. For videos, use full\n "
+			"  path                            For images, use the directory path. For videos or databases, use full\n "
 			"                                  path name\n"
 			"Options:\n"
-			"  -t #.##                         Time threshold (seconds)\n"
+			"  -t #.##                         Time threshold (ms)\n"
 			"  -rate #.##                      Acquisition time (seconds)\n"
 			"  -rateHz #.##                    Acquisition rate (Hz), for convenience\n"
 			"  -repeat #                       Repeat the process on the data set # times (minimum of 1)\n"
-			"  -createGT #                     Generate a ground truth file of dim # (>0, must match the size\n"
-			"                                   of the data set)\n"
+			"  -createGT                       Generate a ground truth file\n"
 			"  -image_width #                  Force an image width (Default 0: original size used).\n"
 			"                                   The height must be also specified if changed.\n"
 			"  -image_height #                 Force an image height (Default 0: original size used)\n"
 			"                                   The height must be also specified if changed.\n"
+			"  -start_at #                     When \"path\" is a directory of images, set this parameter "
+			"                                   to start processing at image # (default 1)."
 			"  -\"parameter name\" \"value\"       Overwrite a specific RTAB-Map's parameter :\n"
 			"                                     -SURF/HessianThreshold 150\n"
 			"                                   For parameters in table format, add ',' between values :\n"
@@ -112,9 +113,10 @@ int main(int argc, char * argv[])
 	float rate = 0.0;
 	int loopDataset = 0;
 	int repeat = 0;
-	int createGT = 0;
+	bool createGT = false;
 	int imageWidth = 0;
 	int imageHeight = 0;
+	int startAt = 1;
 	ParametersMap pm;
 	ULogger::Level logLevel = ULogger::kError;
 	ULogger::Level exitLevel = ULogger::kFatal;
@@ -239,13 +241,13 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-createGT") == 0)
+		if(strcmp(argv[i], "-start_at") == 0)
 		{
 			++i;
 			if(i < argc)
 			{
-				createGT = std::atoi(argv[i]);
-				if(createGT < 1)
+				startAt = std::atoi(argv[i]);
+				if(startAt < 0)
 				{
 					showUsage();
 				}
@@ -256,6 +258,11 @@ int main(int argc, char * argv[])
 			}
 			continue;
 		}
+		if(strcmp(argv[i], "-createGT") == 0)
+		{
+			createGT = true;
+			continue;
+		}
 		if(strcmp(argv[i], "-debug") == 0)
 		{
 			logLevel = ULogger::kDebug;
@@ -299,7 +306,11 @@ int main(int argc, char * argv[])
 				{
 					value = uReplaceChar(value, ',', ' ');
 				}
-				pm.insert(ParametersPair(key, value));
+				std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
+				if(inserted.second == false)
+				{
+					inserted.first->second = value;
+				}
 			}
 			else
 			{
@@ -331,11 +342,19 @@ int main(int argc, char * argv[])
 	Camera * camera = 0;
 	if(UDirectory::exists(path))
 	{
-		camera = new CameraImages(path, 1, false, 0.0f, false, imageWidth, imageHeight);
+		camera = new CameraImages(path, startAt, false, 0.0f, false, imageWidth, imageHeight);
 	}
 	else
 	{
-		camera = new CameraVideo(path, 0.0f, false, imageWidth, imageHeight);
+		std::list<std::string> list = uSplit(path, '.');
+		if(list.size() == 2 && list.back().compare("db") == 0)
+		{
+			camera = new CameraDatabase(path, true, 0.0f, false, imageWidth, imageHeight);
+		}
+		else
+		{
+			camera = new CameraVideo(path, 0.0f, false, imageWidth, imageHeight);
+		}
 	}
 
 	if(!camera || !camera->init())
@@ -344,19 +363,12 @@ int main(int argc, char * argv[])
 		exit(1);
 	}
 
-
-	CvMat * groundTruthMat = 0;
-	if(createGT)
-	{
-		printf("Creating the ground truth matrix...%dx%d\n", createGT, createGT);
-		groundTruthMat = cvCreateMat(createGT, createGT, CV_32FC1);
-	}
-
+	std::map<int, int> groundTruth;
 
 	// Create tasks
 	Rtabmap * rtabmap = new Rtabmap();
 	rtabmap->init();
-	rtabmap->setMaxTimeAllowed(timeThreshold); // in sec
+	rtabmap->setMaxTimeAllowed(timeThreshold); // in ms
 
 	//ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setType(ULogger::kTypeFile, rtabmap->getWorkingDir()+"/LogConsole.txt", false);
@@ -365,7 +377,7 @@ int main(int argc, char * argv[])
 	ULogger::setExitLevel(exitLevel);
 
 	// Disable statistics (we don't need them)
-	pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), uBool2str(false)));
+	pm.insert(ParametersPair(Parameters::kRtabmapPublishStats(), uBool2Str(false)));
 	rtabmap->init(pm);
 
 	printf("Avpd init time = %fs\n", timer.ticks());
@@ -378,10 +390,15 @@ int main(int argc, char * argv[])
 
 	printf("\nParameters : \n");
 	printf(" Data set : %s\n", path.c_str());
-	printf(" Time threshold = %1.2f\n", timeThreshold);
+	printf(" Time threshold = %1.2f ms\n", timeThreshold);
 	printf(" Image rate = %1.2f s (%1.2f Hz)\n", rate, 1/rate);
-	printf(" Repeating dataset = %s\n", repeat?"true":"false");
+	printf(" Repeating data set = %s\n", repeat?"true":"false");
 	printf(" Camera width=%d, height=%d (0 is default)\n", imageWidth, imageHeight);
+	printf(" Camera starts at image %d (default 1)\n", startAt);
+	if(createGT)
+	{
+		printf(" Creating the ground truth matrix.\n");
+	}
 	printf(" INFO: All other parameters are taken from the INI file located in \"~/.rtabmap\"\n");
 	if(pm.size()>1)
 	{
@@ -391,6 +408,10 @@ int main(int argc, char * argv[])
 			printf("    %s=%s\n",iter->first.c_str(), iter->second.c_str());
 		}
 	}
+	if(rtabmap->getWorkingMem().size() || rtabmap->getStMem().size())
+	{
+		printf("[Warning] RTAB-Map database is not empty (%s)\n", (rtabmap->getWorkingDir()+Rtabmap::kDefaultDatabaseName).c_str());
+	}
 	printf("\nProcessing images...\n");
 
 	//setup camera
@@ -453,11 +474,11 @@ int main(int argc, char * argv[])
 			}
 
 			// Update generated ground truth matrix
-			if(groundTruthMat)
+			if(createGT)
 			{
-				if(loopClosureId > 0 && loopClosureId-1 < groundTruthMat->cols)
+				if(loopClosureId > 0)
 				{
-					cvmSet(groundTruthMat, i, loopClosureId-1, 1);
+					groundTruth.insert(std::make_pair(i, loopClosureId-1));
 				}
 			}
 
@@ -475,13 +496,35 @@ int main(int argc, char * argv[])
 					uSleep(delta*1000);
 				}
 			}
-			if(rtabmap->getLoopClosureId())
+			if(actions.size())
 			{
-				printf(" iteration(%d) actions=%d loop(%d) time=%fs\n", count, (int)actions.size(), rtabmap->getLoopClosureId(), iterationTime);
+				if(rtabmap->getLoopClosureId())
+				{
+					printf(" iteration(%d) actions=%d loop(%d) time=%fs *\n", count, (int)actions.size(), rtabmap->getLoopClosureId(), iterationTime);
+				}
+				else if(rtabmap->getReactivatedId())
+				{
+					printf(" iteration(%d) actions=%d high(%d) time=%fs\n", count, (int)actions.size(), rtabmap->getReactivatedId(), iterationTime);
+				}
+				else
+				{
+					printf(" iteration(%d) actions=%d time=%fs\n", count, (int)actions.size(), iterationTime);
+				}
 			}
 			else
 			{
-				printf(" iteration(%d) actions=%d time=%fs\n", count, (int)actions.size(), iterationTime);
+				if(rtabmap->getLoopClosureId())
+				{
+					printf(" iteration(%d) loop(%d) time=%fs *\n", count, rtabmap->getLoopClosureId(), iterationTime);
+				}
+				else if(rtabmap->getReactivatedId())
+				{
+					printf(" iteration(%d) high(%d) time=%fs\n", count, rtabmap->getReactivatedId(), iterationTime);
+				}
+				else
+				{
+					printf(" iteration(%d) time=%fs\n", count, iterationTime);
+				}
 			}
 
 			if(timeThreshold && iterationTime > timeThreshold*100.0f)
@@ -500,47 +543,19 @@ int main(int argc, char * argv[])
 	printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);
 	printf(" Total time = %fs\n", timer.ticks());
 
-	if(groundTruthMat)
+	if(imagesProcessed && createGT)
 	{
-		if(rtabmap->getTotalMemSize() != groundTruthMat->rows)
+		cv::Mat groundTruthMat = cv::Mat::zeros(imagesProcessed, imagesProcessed, CV_8U);
+
+		for(std::map<int, int>::iterator iter = groundTruth.begin(); iter!=groundTruth.end(); ++iter)
 		{
-			printf("WARNING : Ground truth matrix size and the image count don't match : Image captured=%d, GroundTruthSize = %d\n", imagesProcessed, groundTruthMat->rows);
+			groundTruthMat.at<unsigned char>(iter->first, iter->second) = 255;
 		}
 
 		// Generate the ground truth file
-		printf("Generate ground truth to file %s, size of %d\n", (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), groundTruthMat->rows);
-		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("Generate ground truth to file %s, size of %d\n", (rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), groundTruthMat.rows);
+		IplImage img = groundTruthMat;
+		cvSaveImage((rtabmap->getWorkingDir()+GENERATED_GT_NAME).c_str(), &img);
 		printf(" Creating ground truth file = %fs\n", timer.ticks());
 	}
 

corelib/DatabaseViewer/CMakeLists.txt

@@ -18,6 +18,7 @@ QT4_WRAP_CPP(moc_srcs ${headers_ui})
 SET(SRC_FILES 
 	./main.cpp
 	./MainWindow.cpp
+	../../guilib/src/KeypointItem.cpp
 	${moc_srcs} 
 	${moc_uis} 
 )
@@ -26,7 +27,7 @@ SET(INCLUDE_DIRS
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}/../src
 	${CMAKE_CURRENT_SOURCE_DIR}
-	${UTILITE_INCLUDE_DIR}
+	${UTILITE_INCLUDE_DIRS}
 	${OpenCV_INCLUDE_DIRS}
 	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
 )
@@ -34,7 +35,7 @@ SET(INCLUDE_DIRS
 INCLUDE(${QT_USE_FILE})
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
     ${QT_LIBRARIES} 
 	${OpenCV_LIBRARIES} 
 	#${QWT5_LIBRARY} 

corelib/DatabaseViewer/MainWindow.cpp

@@ -31,6 +31,7 @@
 #include <utilite/UTimer.h>
 #include "KeypointMemory.h"
 #include "rtabmap/core/DBDriver.h"
+#include "../../guilib/src/KeypointItem.h"
 
 MainWindow::MainWindow(QWidget * parent) :
 	QMainWindow(parent),
@@ -237,23 +238,70 @@ void MainWindow::generateLocalGraph()
 			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)
+				if(memory_->getSignature(id) > 0)
 				{
-					idsSet.insert(idsSet.end(), iter->first);
+					double dbAccessTime = 0.0;
+					std::map<int, int> ids = memory_->getNeighborsId(dbAccessTime, id, margin, -1, false, false, false);
+					if(ids.size() > 0)
+					{
+						ids.insert(std::pair<int,int>(id, 0));
+						std::set<int> idsSet;
+						for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+						{
+							idsSet.insert(idsSet.end(), iter->first);
+							UINFO("Node %d", iter->first);
+						}
+						UINFO("idsSet=%d", idsSet.size());
+						memory_->generateGraph(path.toStdString(), idsSet);
+					}
+					else
+					{
+						QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
+					}
+				}
+				else
+				{
+					QMessageBox::critical(this, tr("Error"), tr("Signature %1 not found in database.").arg(id));
 				}
-				memory_->generateGraph(path.toStdString(), idsSet);
 			}
 		}
 	}
 }
 
+void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene)
+{
+	if(!scene)
+	{
+		return;
+	}
+	rtabmap::KeypointItem * item = 0;
+	int alpha = 70;
+	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
+	{
+		const cv::KeyPoint & r = (*i).second;
+		int id = (*i).first;
+		QString info = QString( "WordRef = %1\n"
+								"Laplacian = %2\n"
+								"Dir = %3\n"
+								"Hessian = %4\n"
+								"X = %5\n"
+								"Y = %6\n"
+								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
+		float radius = r.size*1.2/9.*2;
+
+		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+
+		scene->addItem(item);
+		item->setZValue(1);
+	}
+}
+
 void MainWindow::sliderAValueChanged(int value)
 {
 	ui_->label_indexA->setText(QString::number(value));
+	ui_->label_actionsA->clear();
+	ui_->label_parentsA->clear();
+	ui_->label_childrenA->clear();
 	if(value >= 0 && value < ids_.size())
 	{
 		ui_->graphicsView_A->scene()->clear();
@@ -261,6 +309,7 @@ void MainWindow::sliderAValueChanged(int value)
 		ui_->label_idA->setText(QString::number(id));
 		if(id>0)
 		{
+			// image
 			QImage img;
 			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
 			if(iter == imagesMap_.end())
@@ -277,7 +326,7 @@ void MainWindow::sliderAValueChanged(int value)
 							QByteArray ba;
 							QBuffer buffer(&ba);
 							buffer.open(QIODevice::WriteOnly);
-							img.save(&buffer, "JPEG"); // writes image into ba in JPEG format
+							img.save(&buffer, "BMP"); // writes image into ba in BMP format
 							imagesMap_.insert(id, ba);
 						}
 					}
@@ -285,7 +334,16 @@ void MainWindow::sliderAValueChanged(int value)
 			}
 			else
 			{
-				img.loadFromData(iter.value(), "JPEG");
+				img.loadFromData(iter.value(), "BMP");
+			}
+
+			if(memory_)
+			{
+				std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
+				if(words.size())
+				{
+					drawKeypoints(words, ui_->graphicsView_A->scene());
+				}
 			}
 
 			if(!img.isNull())
@@ -296,6 +354,61 @@ void MainWindow::sliderAValueChanged(int value)
 			{
 				ULOGGER_DEBUG("Image is empty");
 			}
+
+			// actions
+			if(id-1 > 0)
+			{
+				std::list<rtabmap::NeighborLink> links = memory_->getNeighborLinks(id-1, true, true);
+				for(std::list<rtabmap::NeighborLink>::iterator iter = links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->id()>id-1 && iter->actions().size())
+					{
+						QString str;
+						const std::list<std::vector<float> > & actions = iter->actions();
+						unsigned int j=0;
+						for(std::list<std::vector<float> >::const_iterator jter=actions.begin(); jter!=actions.end(); ++jter)
+						{
+							for(unsigned int i=0; i<jter->size(); ++i)
+							{
+								str.append(QString("%1 ").arg(jter->at(i)));
+							}
+							if(j+1 < actions.size())
+							{
+								str.append(QString("\n"));
+							}
+							++j;
+						}
+						if(str.size())
+						{
+							ui_->label_actionsA->setText(str);
+						}
+						break;
+					}
+				}
+			}
+
+			// loops
+			std::set<int> parents;
+			std::set<int> children;
+			memory_->getLoopClosureIds(id, parents, children, true);
+			if(parents.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				ui_->label_parentsA->setText(str);
+			}
+			if(children.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				ui_->label_childrenA->setText(str);
+			}
 		}
 
 		ui_->label_idA->setText(QString::number(id));
@@ -310,6 +423,9 @@ void MainWindow::sliderAValueChanged(int value)
 void MainWindow::sliderBValueChanged(int value)
 {
 	ui_->label_indexB->setText(QString::number(value));
+	ui_->label_actionsB->clear();
+	ui_->label_parentsB->clear();
+	ui_->label_childrenB->clear();
 	if(value >= 0 && value < ids_.size())
 	{
 		ui_->graphicsView_B->scene()->clear();
@@ -317,6 +433,7 @@ void MainWindow::sliderBValueChanged(int value)
 		ui_->label_idB->setText(QString::number(id));
 		if(id>0)
 		{
+			//image
 			QImage img;
 			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
 			if(iter == imagesMap_.end())
@@ -334,7 +451,7 @@ void MainWindow::sliderBValueChanged(int value)
 							QByteArray ba;
 							QBuffer buffer(&ba);
 							buffer.open(QIODevice::WriteOnly);
-							img.save(&buffer, "JPEG"); // writes image into ba in JPEG format
+							img.save(&buffer, "BMP"); // writes image into ba in BMP format
 							imagesMap_.insert(id, ba);
 						}
 					}
@@ -342,7 +459,16 @@ void MainWindow::sliderBValueChanged(int value)
 			}
 			else
 			{
-				img.loadFromData(iter.value(), "JPEG");
+				img.loadFromData(iter.value(), "BMP");
+			}
+
+			if(memory_)
+			{
+				std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
+				if(words.size())
+				{
+					drawKeypoints(words, ui_->graphicsView_B->scene());
+				}
 			}
 
 			if(!img.isNull())
@@ -353,6 +479,61 @@ void MainWindow::sliderBValueChanged(int value)
 			{
 				ULOGGER_DEBUG("Image is empty");
 			}
+
+			// actions
+			if(id-1 > 0)
+			{
+				std::list<rtabmap::NeighborLink> links = memory_->getNeighborLinks(id-1, true, true);
+				for(std::list<rtabmap::NeighborLink>::iterator iter = links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->id()>id-1 && iter->actions().size())
+					{
+						QString str("");
+						const std::list<std::vector<float> > & actions = iter->actions();
+						unsigned int j=0;
+						for(std::list<std::vector<float> >::const_iterator jter=actions.begin(); jter!=actions.end(); ++jter)
+						{
+							for(unsigned int i=0; i<jter->size(); ++i)
+							{
+								str.append(QString("%1 ").arg(jter->at(i)));
+							}
+							if(j+1 < actions.size())
+							{
+								str.append(QString("\n"));
+							}
+							++j;
+						}
+						if(str.size())
+						{
+							ui_->label_actionsB->setText(str);
+						}
+						break;
+					}
+				}
+			}
+
+			// loops
+			std::set<int> parents;
+			std::set<int> children;
+			memory_->getLoopClosureIds(id, parents, children, true);
+			if(parents.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				ui_->label_parentsB->setText(str);
+			}
+			if(children.size())
+			{
+				QString str;
+				for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+				{
+					str.append(QString("%1 ").arg(*iter));
+				}
+				ui_->label_childrenB->setText(str);
+			}
 		}
 
 		ui_->label_idB->setText(QString::number(id));

corelib/DatabaseViewer/MainWindow.h

@@ -26,13 +26,15 @@
 #include <QtCore/QSet>
 #include <QtGui/QImage>
 #include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
 #include <set>
 
 class Ui_MainWindow;
+class QGraphicsScene;
 
 namespace rtabmap
 {
-class Memory;
+class KeypointMemory;
 }
 
 class MainWindow : public QMainWindow
@@ -57,12 +59,13 @@ private slots:
 private:
 	void updateIds();
 	QImage ipl2QImage(const IplImage *newImage);
+	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene);
 
 private:
 	Ui_MainWindow * ui_;
 	QMap<int, QByteArray> imagesMap_;
 	QList<int> ids_;
-	rtabmap::Memory * memory_;
+	rtabmap::KeypointMemory * memory_;
 	QString pathDatabase_;
 };
 

corelib/DatabaseViewer/ui/MainWindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>550</width>
-    <height>318</height>
+    <width>600</width>
+    <height>350</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -20,6 +20,52 @@
       <item>
        <widget class="QGraphicsView" name="graphicsView_A"/>
       </item>
+      <item>
+       <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_actionsA">
+          <property name="text">
+           <string>Actions</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="label_parentsA">
+          <property name="text">
+           <string>Parents</string>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QLabel" name="label_actionsA_2">
+          <property name="text">
+           <string>Actions</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="0">
+         <widget class="QLabel" name="label_parentsA_2">
+          <property name="text">
+           <string>Parents</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QLabel" name="label_childrenA_2">
+          <property name="text">
+           <string>Children</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_childrenA">
+          <property name="text">
+           <string>Children</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
       <item>
        <layout class="QHBoxLayout" name="horizontalLayout">
         <item>
@@ -77,6 +123,52 @@
       <item>
        <widget class="QGraphicsView" name="graphicsView_B"/>
       </item>
+      <item>
+       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
+        <item row="0" column="1">
+         <widget class="QLabel" name="label_actionsB">
+          <property name="text">
+           <string>Actions</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QLabel" name="label_parentsB">
+          <property name="text">
+           <string>Parents</string>
+          </property>
+         </widget>
+        </item>
+        <item row="0" column="0">
+         <widget class="QLabel" name="label_actionsA_4">
+          <property name="text">
+           <string>Actions</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="0">
+         <widget class="QLabel" name="label_parentsA_4">
+          <property name="text">
+           <string>Parents</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="0">
+         <widget class="QLabel" name="label_childrenA_3">
+          <property name="text">
+           <string>Children</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QLabel" name="label_childrenB">
+          <property name="text">
+           <string>Children</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
       <item>
        <layout class="QHBoxLayout" name="horizontalLayout_2">
         <item>
@@ -136,8 +228,8 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>550</width>
-     <height>22</height>
+     <width>600</width>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">

corelib/ImagesDbExtractor/CMakeLists.txt

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

corelib/ImagesDbExtractor/main.cpp

@@ -0,0 +1,61 @@
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types_c.h>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
+#include <iostream>
+#include <utilite/ULogger.h>
+#include <utilite/UTimer.h>
+#include <utilite/UConversion.h>
+#include <utilite/UDirectory.h>
+#include "SMMemory.h"
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/LTM.db";
+	if(argc > 1)
+	{
+		path = argv[1];
+	}
+
+
+    // Open database
+    std::string driverType = "sqlite3";
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
+	rtabmap::SMMemory * memory = new rtabmap::SMMemory(parameters);
+	if(!memory)
+	{
+		UWARN("Can't create database driver \"%s\"", driverType.c_str());
+	}
+	else if(!memory->init(driverType, path))
+	{
+		UWARN("Can't open database \"%s\"", path.c_str());
+	}
+
+	if(memory)
+	{
+		UINFO("Using database %s", path.c_str());
+		std::string saveDirectory = "imagesExtracted/";
+		if(!UDirectory::exists(saveDirectory))
+		{
+			UDirectory::makeDir(saveDirectory);
+		}
+		std::set<int> ids = memory->getAllSignatureIds();
+		for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			IplImage * image = memory->getImage(*iter);
+			if(image)
+			{
+				std::string fileName = uNumber2Str(*iter) + ".jpg";
+				cvSaveImage((saveDirectory+fileName).c_str(), image);
+				cvReleaseImage(&image);
+				UINFO("Saved %s", (saveDirectory+fileName).c_str());
+			}
+		}
+	}
+
+    return 0;
+}

corelib/ImagesJoiner/CMakeLists.txt

@@ -5,12 +5,12 @@ SET(SRC_FILES
 
 SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBS} 
 )
 

corelib/ImagesJoiner/main.cpp

@@ -84,16 +84,16 @@ int main(int argc, char * argv[])
 
 	while(imagesExist)
 	{
-		std::string fileNameTarget = uNumber2str(counterJoined);
+		std::string fileNameTarget = uNumber2Str(counterJoined);
 		if(inv)
 		{
-			fileNameA =  uNumber2str(counterImages+1);
-			fileNameB =  uNumber2str(counterImages);
+			fileNameA =  uNumber2Str(counterImages+1);
+			fileNameB =  uNumber2Str(counterImages);
 		}
 		else
 		{
-			fileNameA =  uNumber2str(counterImages);
-			fileNameB =  uNumber2str(counterImages+1);
+			fileNameA =  uNumber2Str(counterImages);
+			fileNameB =  uNumber2Str(counterImages+1);
 		}
 
 		while(fileNameA.size() < sizeFileName)

corelib/LogPolar/CMakeLists.txt

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

corelib/LogPolar/main.cpp

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

corelib/WebcamCapture/CMakeLists.txt

@@ -5,13 +5,13 @@ SET(SRC_FILES
 
 SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
     ${CMAKE_CURRENT_SOURCE_DIR}/../include
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBS} 
 )
 

corelib/WebcamCapture/main.cpp

@@ -92,7 +92,7 @@ protected:
 				if(save_)
 				{
 					std::string fileName = targetDir_ + "/";
-					fileName += uNumber2str(id_++);
+					fileName += uNumber2Str(id_++);
 					fileName += ".";
 					fileName += ext_;
 					cvSaveImage(fileName.c_str(), image);
@@ -225,11 +225,11 @@ int main(int argc, char * argv[])
 		   imageWidth, 
 		   imageHeight, 
 		   imageRate, 
-		   uBool2str(show).c_str(), 
+		   uBool2Str(show).c_str(),
 		   targetDirectory.c_str(),
 		   extension.c_str(),
 		   startId,
-		   uBool2str(save).c_str());
+		   uBool2Str(save).c_str());
 
 	UDirectory::makeDir(targetDirectory);
 

corelib/include/rtabmap/core/Camera.h

@@ -27,6 +27,8 @@
 #include "rtabmap/core/Parameters.h"
 #include <set>
 #include <stack>
+#include <list>
+#include <vector>
 
 class UDirectory;
 
@@ -57,8 +59,8 @@ public:
 class RTABMAP_EXP CamKeypointTreatment : public CamPostTreatment
 {
 public:
-	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
-	enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
+	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
+	enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
 
 public:
 	CamKeypointTreatment(const ParametersMap & parameters = ParametersMap()) :
@@ -90,13 +92,16 @@ public:
 public:
 	virtual ~Camera();
 
-	virtual SMState * takeImage() = 0;
+	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0) = 0;
+	SMState * takeSMState();
 	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
+	void setImageRate(float imageRate) {_imageRate = imageRate;}
+	void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
 
 protected:
 	/**
@@ -145,7 +150,7 @@ public:
 			unsigned int imageHeight = 0);
 	virtual ~CameraImages();
 
-	virtual SMState * takeImage();
+	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
 	virtual bool init();
 
 private:
@@ -187,7 +192,7 @@ public:
 			unsigned int imageHeight = 0);
 	virtual ~CameraVideo();
 
-	virtual SMState * takeImage();
+	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
 	virtual bool init();
 
 private:
@@ -215,19 +220,19 @@ class RTABMAP_EXP CameraDatabase :
 {
 public:
 	CameraDatabase(const std::string & path,
-			bool ignoreChildren,
+			bool loadActions,
 			float imageRate = 0,
 			bool autoRestart = false,
 			unsigned int imageWidth = 0,
 			unsigned int imageHeight = 0);
 	virtual ~CameraDatabase();
 
-	virtual SMState * takeImage();
+	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
 	virtual bool init();
 
 private:
 	std::string _path;
-	bool _ignoreChildren;
+	bool _loadActions;
 	std::set<int>::iterator _indexIter;
 	DBDriver * _dbDriver;
 	std::set<int> _ids;

corelib/include/rtabmap/core/CameraEvent.h

@@ -33,45 +33,18 @@ class RTABMAP_EXP CameraEvent :
 {
 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)
+		UEvent(kCodeNoMoreImages)
 	{
-
 	}
 
 	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

corelib/include/rtabmap/core/DBDriver.h

@@ -30,11 +30,12 @@
 #include "utilite/UMutex.h"
 #include "utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/Signature.h"
 
 namespace rtabmap {
 
-class Signature;
 class KeypointSignature;
+class SMSignature;
 class VWDictionary;
 class VisualWord;
 
@@ -52,6 +53,7 @@ class RTABMAP_EXP DBDriver : public UThreadNode
 public:
 	virtual ~DBDriver();
 
+	virtual std::string getDriverName() const = 0;
 	virtual void parseParameters(const ParametersMap & parameters);
 	const std::string & getUrl() const {return _url;}
 
@@ -62,6 +64,7 @@ public:
 	void asyncSave(VisualWord * s);
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+	bool isImagesCompressed() const {return _imagesCompressed;}
 
 public:
 	bool addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
@@ -71,15 +74,12 @@ public:
 	bool deleteAllObsoleteSSVWLinks() const;
 	bool deleteUnreferencedWords() const;
 
-	bool addNeighbor(int id, int newNeighbor, int oldNeighbor);
-	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);
+	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection();
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
@@ -93,28 +93,27 @@ public:
 	// 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 loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
+	bool loadSMSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
 	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 getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const;
+	bool loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const;
 	bool getWeight(int signatureId, int & weight) const;
-	bool getLoopClosureId(int signatureId, int & loopId) const;
-	bool getImageCompressed(int id, CvMat ** compressed) const;
+	bool getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) 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 bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery() = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
@@ -123,15 +122,13 @@ private:
 
 	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 getNeighborIdsQuery(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) 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 newNeighbor, int oldNeighbor) const = 0;
+	virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) 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;
+	virtual bool saveQuery(const std::list<Signature *> & signatures) const = 0;
 
 	// Load objects
 	virtual bool loadQuery(VWDictionary * dictionary) const = 0;
@@ -139,16 +136,17 @@ private:
 	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 loadQuery(int signatureId, SMSignature * ss) const = 0;
+	virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
+	virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) 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 loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const = 0;
 
-	virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const = 0;
+	virtual bool getImageQuery(int id, IplImage ** image) 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:
@@ -168,6 +166,7 @@ private:
 	USemaphore _addSem;
 	unsigned int _minSignaturesToSave;
 	unsigned int _minWordsToSave;
+	bool _imagesCompressed;
 	bool _asyncWaiting;
 	double _emptyTrashesTime;
 	std::string _url;

corelib/include/rtabmap/core/KeypointDescriptor.h

@@ -33,89 +33,75 @@ 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);
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const = 0;
 
 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;
+	KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
 };
 
 //SURFDescriptor
 class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
 {
 public:
-	SURFDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	SURFDescriptor(const ParametersMap & parameters = ParametersMap());
 	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;
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	cv::SURF _surf;
+	CvSURFParams _params;
 	bool _gpuVersion;
-	bool _upright;
 };
 
 //SIFTDescriptor
 class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
 {
 public:
-	SIFTDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
 	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;
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
 	cv::SIFT::CommonParams _commonParams;
 	cv::SIFT::DescriptorParams _descriptorParams;
 };
 
-//LaplacianDescriptor
-class RTABMAP_EXP LaplacianDescriptor : public KeypointDescriptor
+//BRIEFDescriptor
+class RTABMAP_EXP BRIEFDescriptor : public KeypointDescriptor
 {
 public:
-	LaplacianDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
-	virtual ~LaplacianDescriptor();
+	BRIEFDescriptor(const ParametersMap & parameters = ParametersMap());
+	virtual ~BRIEFDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+
 private:
-	virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
+	int _size;
 };
 
 //MinMax ColorDescriptor
 class RTABMAP_EXP ColorDescriptor : public KeypointDescriptor
 {
 public:
-	ColorDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
+	ColorDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~ColorDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
 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);
+	HueDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~HueDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
 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;
 

corelib/include/rtabmap/core/KeypointDetector.h

@@ -37,19 +37,19 @@ class RTABMAP_EXP KeypointDetector
 {
 public:
 	virtual ~KeypointDetector() {}
-	std::list<cv::KeyPoint> generateKeypoints(const IplImage * image);
+	std::vector<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);
+	cv::Rect computeRoi(const IplImage * image) const;
 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;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const = 0;
 private:
 	unsigned int _wordsPerImageTarget;
 	bool _usingAdaptiveResponseThr;
@@ -64,13 +64,12 @@ public:
 	SURFDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURFDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return _surf.hessianThreshold;};
+	virtual double getMinimumResponseThr() const {return _params.hessianThreshold;};
 private:
-	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
 private:
-	cv::SURF _surf;
+	CvSURFParams _params;
 	bool _gpuVersion;
-	bool _upright;
 };
 
 //SIFTDetector
@@ -82,7 +81,7 @@ public:
 	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;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
 private:
 	cv::SIFT::CommonParams _commonParams;
 	cv::SIFT::DetectorParams _detectorParams;
@@ -95,11 +94,26 @@ public:
 	StarDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~StarDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return (double)_star.responseThreshold;};
+	virtual double getMinimumResponseThr() const {return (double)_params.responseThreshold;};
 private:
-	virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
 private:
-	cv::StarDetector _star;
+	CvStarDetectorParams _params;
+};
+
+//FASTDetector
+class RTABMAP_EXP FASTDetector : public KeypointDetector
+{
+public:
+	FASTDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~FASTDetector();
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual double getMinimumResponseThr() const {return (double)_threshold;};
+private:
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+private:
+	int _threshold;
+	bool _nonmaxSuppression;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -25,7 +25,6 @@
 #include "utilite/UEvent.h"
 #include <string>
 #include <map>
-#include "utilite/UDestroyer.h"
 
 namespace rtabmap
 {
@@ -121,35 +120,40 @@ typedef std::pair<const std::string, std::string> ParametersPair;
 class RTABMAP_EXP Parameters
 {
 	// Rtabmap parameters
-	RTABMAP_PARAM(Rtabmap, VhStrategy, 	        int, 0);	   // None 0, Similarity 1, Epipolar 2
+	RTABMAP_PARAM(Rtabmap, VhStrategy, 	         int, 0);	   // None 0, Similarity 1, Epipolar 2
 	RTABMAP_PARAM(Rtabmap, PublishStats, 	     bool, true); // Publishing statistics
-	RTABMAP_PARAM(Rtabmap, RetrievalThr,     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, PublishImages, 	     bool, true); // Publishing images
+	RTABMAP_PARAM(Rtabmap, PublishPdf, 	         bool, true); // Publishing pdf
+	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	 bool, true); // Publishing likelihood
+	RTABMAP_PARAM(Rtabmap, RetrievalThr,         float, 0.0); // Reactivation threshold
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		     float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
+	RTABMAP_PARAM(Rtabmap, MemoryThr, 		     int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
 	RTABMAP_PARAM(Rtabmap, SMStateBufferSize,    int, 1);      // Data buffer size (0 min inf)
-	RTABMAP_PARAM(Rtabmap, MinMemorySizeForLoopDetection, unsigned int, 25); //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
-	RTABMAP_PARAM(Rtabmap, ActionsByTime,       bool, true); // Select next actions using directly the more recent neighbor of the current node, otherwise, highest hypothesis is used
+	RTABMAP_PARAM(Rtabmap, SelectionNeighborhoodSummationUsed, bool, false); // Neighborhood summation for hypothesis selection
+	RTABMAP_PARAM(Rtabmap, SelectionLikelihoodUsed, bool, false); // Neighborhood likelihood for hypothesis selection
 	RTABMAP_PARAM(Rtabmap, ActionsSentRejectHyp,  bool, true); // Actions sent also on rejected hypotheses (on decreasing hypotheses)
 	RTABMAP_PARAM(Rtabmap, ConfidenceThr, 		float, 0.0); // Actions are not sent when the loop closure hypothesis is under the confidence threshold
+	RTABMAP_PARAM(Rtabmap, LikelihoodStdDevRemoved, bool, true); // Remove std dev on likelihood normalization.
 
 	// 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
+	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.15); // Loop closing threshold
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0); // 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, true); // Keep raw data
-	RTABMAP_PARAM(Mem, MaxStMemSize, 		   unsigned int, 25); // Short-time memory size
+	RTABMAP_PARAM(Mem, RawDataKept, 		   bool, false); // Keep raw data
+	RTABMAP_PARAM(Mem, MaxStMemSize, 		   unsigned int, 30); // 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
+	RTABMAP_PARAM(Mem, DataMergedOnRehearsal,  bool, true); // Merge data on rehearsal
+	RTABMAP_PARAM(Mem, SignatureType, 		   int, 0); 	 // Keypoint 0, Sensorimotor 1
 
 	// KeypointMemory (Keypoint-based)
+	RTABMAP_PARAM(Kp, PublishKeypoints, 	 bool, true); // Publishing keypoints
 	RTABMAP_PARAM(Kp, NNStrategy,     	int, 2);	  // Naive 0, kdTree 1, kdForest 2
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
 	RTABMAP_PARAM(Kp, WordsPerImage,    int, 400);
@@ -159,24 +163,34 @@ class RTABMAP_EXP Parameters
 	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, DetectorStrategy, int, 0);      // Surf detector 0, Star detector 1, SIFT detector 2, FAST detector 3
+	RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0);      // kDescriptorSurf=0, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, 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, SensorStateOnly, bool, true); // If using only sensors state (without actuators) for sensorimotor state nearest neighbor computation
 	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
 
+	// SM memory
+	RTABMAP_PARAM(SM, PublishMasks, 	 bool, true); // Publishing motion masks
+	RTABMAP_PARAM(SM, MotionMaskUsed, bool, false); // Use motion mask
+	RTABMAP_PARAM(SM, LogPolarUsed, bool, false); // Use log-polar images
+	RTABMAP_PARAM(SM, VotingSchemeUsed, bool, false); // Use likelihood voting scheme
+	RTABMAP_PARAM(SM, ColorTable, int, 8); // Color table size 0=8, 1=16, 2=32, 3=64, 4=128, 5=256, 6=512, 7=1024, 8=65536
+
 	//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(Db, ImagesCompressed,     bool, true);	 	 // Images are compressed when save to database
 	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(DbSqlite3, JournalMode,  int, 3);	 	 // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
+	RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0);	 	 // 0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : "PRAGMA synchronous")
+	RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2);	 	 // 0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : "PRAGMA temp_store")
 
+	// Keypoints descriptors/detectors
 	RTABMAP_PARAM(SURF, Extended, 		  bool, false); // true=128, false=64
 	RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0);
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4);
@@ -187,6 +201,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SIFT, Threshold, 	double, 0.006667); // true=128, false=64
 	RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
 
+	RTABMAP_PARAM(FAST, Threshold, 	int, 10);
+	RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true);
+
+	RTABMAP_PARAM(BRIEF, Size, 	int, 32); // 16, 32, 64
+
 	RTABMAP_PARAM(Star, MaxSize, int, 45);
 	RTABMAP_PARAM(Star, ResponseThreshold, int, 30);
 	RTABMAP_PARAM(Star, LineThresholdProjected, int, 10);
@@ -195,7 +214,8 @@ class RTABMAP_EXP Parameters
 
 	// 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, ...}
+	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23"); // Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}
+	RTABMAP_PARAM(Bayes, PredictionOnNonNullActionsOnly, bool, false); // Make prediction on non-null action neighbors only
 
 	// Verify hypotheses
 	RTABMAP_PARAM(Vh,   Similarity,    float, 0.5); // Minimum similarity to accept an hypothesis
@@ -209,15 +229,11 @@ public:
 
 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_;
+	static Parameters instance_;
 };
 
 /**

corelib/include/rtabmap/core/Rtabmap.h

@@ -64,6 +64,7 @@ public:
 
 	static const char * kDefaultIniFileName;
 	static const char * kDefaultIniFilePath;
+	static const char * kDefaultDatabaseName;
 
 public:
 	static std::string getVersion();
@@ -84,6 +85,7 @@ public:
 
 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const;
+	int getReactivatedId() const;
 	int getLastSignatureId() const;
 	const std::list<std::vector<float> > & getActions() const {return _actions;}
 	std::list<int> getWorkingMem() const;
@@ -92,15 +94,16 @@ public:
 	int getTotalMemSize() const;
 	const std::string & getGraphFileName() const {return _graphFileName;}
 
-	void setMaxTimeAllowed(float maxTimeAllowed); // in sec
+	void setMaxTimeAllowed(float maxTimeAllowed); // in ms
 	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;
+	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
+											const std::map<int, float> & likelihood,
+											bool neighborSumUsed,
+											bool likelihoodUsed) const;
 
 protected:
 	virtual void handleEvent(UEvent * anEvent);
@@ -110,8 +113,7 @@ private:
 	virtual void killCleanup();
 	virtual void startInit();
 	void process();
-	void resetMemory();
-	void deleteMemory();
+	void resetMemory(bool dbOverwritten = false);
 	void addSMState(SMState * data); // ownership is transferred
 	SMState * getSMState();
 	void setupLogFiles(bool overwrite = false);
@@ -123,22 +125,27 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	float _maxTimeAllowed; // in sec
+	bool _publishImages;
+	bool _publishPdf;
+	bool _publishLikelihood;
+	bool _publishKeypoints;
+	bool _publishMasks;
+	float _maxTimeAllowed; // in ms
+	unsigned int _maxMemoryAllowed; // signatures count in WM
 	int _smStateBufferMaxSize;
-	unsigned int _minMemorySizeForLoopDetection;
 	float _loopThr;
 	float _loopRatio;
 	float _retrievalThr;
-	bool _localGraphCleaned;
 	unsigned int _maxRetrieved;
-	bool _actionsByTime;
+	bool _selectionNeighborhoodSummationUsed;
+	bool _selectionLikelihoodUsed;
 	bool _actionsSentRejectHyp;
 	float _confidenceThr;
+	bool _likelihoodStdDevRemoved;
 
 	int _lcHypothesisId;
 	int _reactivateId;
-	float _highestHypothesisValue;
-	unsigned int _spreadMargin;
+	float _lastLcHypothesisValue;
 	int _lastLoopClosureId;
 	std::list<std::vector<float> > _actions;
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -45,22 +45,23 @@ namespace rtabmap
 
 class RTABMAP_EXP Statistics
 {
-	RTABMAP_STATS(Loop, Closure_id,);
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
+	RTABMAP_STATS(Loop, Vp_hypothesis,);
 	RTABMAP_STATS(Loop, Vp_likelihood,);
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
-	RTABMAP_STATS(Loop, Retrieval_margin,);
 	RTABMAP_STATS(Loop, Actions,);
+	RTABMAP_STATS(Loop, Actions_of,);
+	RTABMAP_STATS(Loop, Actions_chosen,);
 
 	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, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
@@ -70,11 +71,13 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Posterior_computation, ms);
 	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
 	RTABMAP_STATS(Timing, Hypotheses_validation, ms);
+	RTABMAP_STATS(Timing, Action_selection, 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(Timing, Joining_trash, ms);
+	RTABMAP_STATS(Timing, Emptying_trash, ms);
 
 	RTABMAP_STATS(, Parent_id,);
 	RTABMAP_STATS(, Hypothesis_reactivated,);
@@ -107,6 +110,8 @@ public:
 	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;}
+	void setRefMotionMask(const std::vector<unsigned char> & mask) {_refMotionMask = mask;}
+	void setLoopMotionMask(const std::vector<unsigned char> & mask) {_loopMotionMask = mask;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -120,6 +125,9 @@ public:
 	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::vector<unsigned char> & refMotionMask() const {return _refMotionMask;}
+	const std::vector<unsigned char> & loopMotionMask() const {return _loopMotionMask;}
+
 	const std::map<std::string, float> & data() const {return _data;}
 
 
@@ -141,10 +149,14 @@ private:
 	std::map<int, float> _posterior;
 	std::map<int, float> _likelihood;
 
-	//surf
+	//keypoint memory
 	std::multimap<int, cv::KeyPoint> _refWords;
 	std::multimap<int, cv::KeyPoint> _loopWords;
 
+	//sm memory
+	std::vector<unsigned char> _refMotionMask;
+	std::vector<unsigned char> _loopMotionMask;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/SMState.h

@@ -23,6 +23,7 @@
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui_c.h>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include <vector>
@@ -39,7 +40,7 @@ public:
 		_image(image)
 	{}
 	// Constructor 1
-	SMState(const std::list<std::vector<float> > & sensors, const std::list<std::vector<float> > & actuators) :
+	SMState(const cv::Mat & sensors, const std::list<std::vector<float> > & actuators) :
 		_sensors(sensors),
 		_actuators(actuators),
 		_image(0)
@@ -76,12 +77,12 @@ public:
 	}
 
 	const IplImage * getImage() const {return _image;}
-	const std::list<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
-	const std::list<std::vector<float> > & getSensors() const {return _sensors;}
+	const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
+	const cv::Mat & getSensors() const {return _sensors;}
 	const std::list<std::vector<float> > & getActuators() const {return _actuators;}
-	void setSensors(const std::list<std::vector<float> > & sensors) {_sensors=sensors;}
+	void setSensors(const cv::Mat & sensors) {_sensors=sensors;}
 	void setActuators(const std::list<std::vector<float> > & actuators) {_actuators=actuators;}
-	void setKeypoints(const std::list<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
+	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
 
 	//ownership is transferred
 	void setImage(IplImage * image)
@@ -97,15 +98,15 @@ public:
 	{
 		sensors.clear();
 		step = 0;
-		if(_sensors.size())
+		if(!_sensors.empty())
 		{
 			// here we assume that all sensors have the same length
-			step = _sensors.front().size();
-			for(std::list<std::vector<float> >::const_iterator iter = _sensors.begin();
-					iter != _sensors.end();
-					++iter)
+			step = _sensors.cols;
+			sensors = std::vector<float>(_sensors.total());
+			for(int i=0; i<_sensors.rows; ++i)
 			{
-				sensors.insert(sensors.end(), iter->begin(), iter->end());
+				const float * rowFl = _sensors.ptr<float>(i);
+				memcpy(&sensors[i*_sensors.cols], rowFl, _sensors.cols*sizeof(float));
 			}
 		}
 	}
@@ -127,10 +128,10 @@ public:
 	}
 
 private:
-	std::list<std::vector<float> > _sensors; // descriptors
+	cv::Mat _sensors; // descriptors
 	std::list<std::vector<float> > _actuators;
 	IplImage * _image;
-	std::list<cv::KeyPoint> _keypoints;
+	std::vector<cv::KeyPoint> _keypoints;
 };
 
 // Sensorimotor state event

corelib/include/rtabmap/core/Signature.h

@@ -0,0 +1,191 @@
+/*
+ * 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>
+#include <set>
+
+//TODO : add copy constructor
+
+namespace rtabmap
+{
+
+class RTABMAP_EXP NeighborLink
+{
+public:
+	NeighborLink(int id, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >(), const std::vector<int> & baseIds = std::vector<int>()) :
+		_id(id),
+		_actions(actions),
+		_baseIds(baseIds)
+	{}
+	virtual ~NeighborLink() {}
+
+	int id() const {return _id;}
+	const std::list<std::vector<float> > & actions() const {return _actions;}
+	const std::vector<int> & baseIds() const {return _baseIds;}
+	bool updateIds(int idFrom, int idTo);
+
+private:
+	int _id;
+	std::list<std::vector<float> > _actions;
+	std::vector<int> _baseIds; // first is the nearest
+};
+
+class Memory;
+typedef std::multimap<int, NeighborLink> NeighborsMultiMap;
+
+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 NeighborsMultiMap & neighbors);
+	void addNeighbor(const NeighborLink & neighbor);
+	void removeNeighbor(int neighborId) {if(_neighbors.erase(neighborId)) _neighborsModified = true;}
+	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
+	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
+	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_modified=true;}
+	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_modified=true;}
+	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_modified=true;}
+	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
+	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_modified=true;}
+	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_modified=true;}
+	void setSaved(bool saved) {_saved = saved;}
+	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
+	void changeNeighborIds(int idFrom, int idTo);
+
+	const NeighborsMultiMap & getNeighbors() const {return _neighbors;}
+	int getWeight() const {return _weight;}
+	const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
+	const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
+	bool isSaved() const {return _saved;}
+	bool isModified() const {return _modified || _neighborsModified;}
+	bool isNeighborsModified() const {return _neighborsModified;}
+
+protected:
+	Signature(int id, const IplImage * image = 0, bool keepImage = false);
+
+private:
+	int _id;
+	NeighborsMultiMap _neighbors; // id, neighborLink
+	int _weight;
+	std::set<int> _loopClosureIds;
+	std::set<int> _childLoopClosureIds;
+	IplImage * _image;
+	bool _saved; // If it's saved to bd
+	bool _modified;
+	bool _neighborsModified; // Optimization when updating signatures in database
+};
+
+
+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;}
+	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
+
+
+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>
+	std::map<int, int> _wordsChanged; // <oldId, newId>
+	bool _enabled;
+
+};
+
+class RTABMAP_EXP SMSignature :
+	public Signature
+{
+public:
+	SMSignature(
+		const std::vector<int> & sensors,
+		const std::vector<unsigned char> & motionMask,
+		int id,
+		const IplImage * image = 0,
+		bool keepRawData = false);
+	SMSignature(int id);
+
+	virtual ~SMSignature();
+
+	virtual float compareTo(const Signature * signature) const;
+	virtual bool isBadSignature() const;
+	virtual std::string signatureType() const {return "SMSignature";};
+
+	void setSensors(const std::vector<int> & sensors) {_sensors= sensors;}
+	const std::vector<int> & getSensors() const {return _sensors;}
+
+	void setMotionMask(const std::vector<unsigned char> & motionMask) {_motionMask= motionMask;}
+	const std::vector<unsigned char> & getMotionMask() const {return _motionMask;}
+private:
+	std::vector<int> _sensors;
+	std::vector<unsigned char> _motionMask;
+};
+
+
+} // namespace rtabmap

corelib/src/BayesFilter.cpp

@@ -19,15 +19,17 @@
 
 #include "BayesFilter.h"
 #include "Memory.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
+#include <iostream>
 
 #include "utilite/UtiLite.h"
 
 namespace rtabmap {
 
 BayesFilter::BayesFilter(const ParametersMap & parameters) :
-	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr())
+	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
+	_predictionOnNonNullActionsOnly(Parameters::defaultBayesPredictionOnNonNullActionsOnly())
 {
 	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
 	this->parseParameters(parameters);
@@ -47,6 +49,11 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
 	{
 		this->setPredictionLC((*iter).second);
 	}
+	if((iter=parameters.find(Parameters::kBayesPredictionOnNonNullActionsOnly())) != parameters.end())
+	{
+		_predictionOnNonNullActionsOnly = uStr2Bool((*iter).second.c_str());
+	}
+
 }
 
 void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
@@ -73,23 +80,18 @@ 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());
+		UERROR("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)
+			//UINFO("%d=%e", i, tmpValues[i]);
+			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
 			{
 				valid = false;
 				break;
@@ -97,9 +99,9 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 			++i;
 		}
 
-		if(!valid || sum <= 0 || sum > 1.001)
+		if(!valid)
 		{
-			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());
+			UERROR("The prediction is not valid (values must be between >0 && <=1) prediction=\"%s\"", prediction.c_str());
 		}
 		else
 		{
@@ -119,7 +121,7 @@ std::string BayesFilter::getPredictionLCStr() const
 	std::string values;
 	for(unsigned int i=0; i<_predictionLC.size(); ++i)
 	{
-		values.append(uNumber2str(_predictionLC[i]));
+		values.append(uNumber2Str(_predictionLC[i]));
 		if(i+1 < _predictionLC.size())
 		{
 			values.append(" ");
@@ -167,16 +169,10 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	// 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))
+	if(this->generatePrediction(prediction, memory, uKeys(likelihood)))
 	{
 		ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction->rows, prediction->cols);
+		//std::cout << "Prediction=" << cv::Mat(prediction) << std::endl;
 
 		// Adjust the last posterior if some images were
 		// reactivated or removed from the working memory
@@ -188,12 +184,18 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 			posterior->data.fl[j++] = (*i).second;
 		}
 		ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior->rows, _posterior.size());
+		//std::cout << "LastPosterior=" << cv::Mat(posterior) << std::endl;
 
 		// 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());
+		//std::cout << "ResultingPrior=" << cv::Mat(prior) << std::endl;
+
+		ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
+		std::vector<float> likelihoodValues = uValues(likelihood);
+		//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
 
 		// STEP 2 - Update : Multiply with observations (likelihood)
 		j=0;
@@ -231,7 +233,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	return _posterior;
 }
 
-bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const
+bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::vector<int> & ids) const
 {
 	ULOGGER_DEBUG("");
 	UTimer timer;
@@ -239,97 +241,108 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
 	UTimer timerGlobal;
 	timerGlobal.start();
 
-	if(!likelihoodIds.size() ||
+	if(!memory ||
 	   prediction == 0 ||
 	   prediction->rows != prediction->cols ||
-	   (unsigned int)prediction->rows != likelihoodIds.size()/*||
-	   prediction->type != CV_32FC1*/ ||
+	   (unsigned int)prediction->rows != ids.size() ||
 	   _predictionLC.size() < 2 ||
-	   !memory)
+	   !ids.size())
 	{
 		ULOGGER_ERROR( "fail");
 		return false;
 	}
 
+	std::map<int, int> idToIndexMap;
+	for(unsigned int i=0; i<ids.size(); ++i)
+	{
+		if(ids[i] == 0)
+		{
+			UFATAL("Signature id is null ?!?");
+		}
+		idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
+	}
+
 	//int rows = prediction->rows;
 	cvSetZero(prediction);
 	int cols = prediction->cols;
 
-	// Each priors are column vectors
-	unsigned int i=0;
+	// Each prior is a column vector
 	ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
-	for(std::map<int, int>::const_iterator iter=likelihoodIds.begin(); iter!=likelihoodIds.end(); ++iter)
+	for(unsigned int i=0; i<ids.size(); ++i)
 	{
-		if(iter->first > 0)
+		int loopSignId = ids[i];
+		if(loopSignId > 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)
+
+			float sum = 0.0f; // sum values added
+
+			float totalModelValues = 0.0f;
+			for(unsigned int j=0; j<_predictionLC.size(); ++j)
 			{
-				ULOGGER_ERROR("loopSign %d is not found?!?", loopClosureId);
+				totalModelValues += _predictionLC[j];
 			}
 
-			// 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)
+			// ADD prob for each neighbors
+			double dbAccessTime = 0.0;
+			std::map<int, int> neighbors = memory->getNeighborsId(dbAccessTime, loopSignId, _predictionLC.size()-1, 0, _predictionOnNonNullActionsOnly);
+			sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
+			// ADD values of not found neighbors to loop closure
+			if(sum < totalModelValues-_predictionLC[0])
 			{
-				totalModelValues += _predictionLC[j]*2;
+				float delta = totalModelValues-_predictionLC[0]-sum;
+				prediction->data.fl[i + i*cols] += delta;
+				sum+=delta;
 			}
 
-			//Add values of not found neighbors to the loop closure
-			float sum = 0;
-			for(int j=0; j<cols; ++j)
+			float allOtherPlacesValue = 0;
+			if(totalModelValues < 1)
 			{
-				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];
+				allOtherPlacesValue = 1.0f - totalModelValues;
 			}
 
 			// Set all loop events to small values according to the model
-			if(totalModelValues < 1.0f)
+			if(allOtherPlacesValue > 0 && cols>1)
 			{
-				float value = (1.0f-totalModelValues) / float(cols);
-				for(int j=0; j<cols; ++j)
+				float value = allOtherPlacesValue / float(cols - 1);
+				for(int j=ids[0] < 0?1:0; j<cols; ++j)
 				{
-					if(!prediction->data.fl[i + j*cols])
+					if(prediction->data.fl[i + j*cols] == 0)
 					{
-						sum += prediction->data.fl[i + j*cols] = value;
+						prediction->data.fl[i + j*cols] = value;
+						sum += prediction->data.fl[i + j*cols];
 					}
 				}
 			}
 
-			//normalize this row,
-			for(int j=0; j<cols; ++j)
+			//normalize this row
+			float maxNorm = 1 - (ids[0]<0?_predictionLC[0]:0); // 1 - virtual place probability
+			if(sum<maxNorm-0.0001 || sum>maxNorm+0.0001)
 			{
-				prediction->data.fl[i + j*cols] /= sum;
+				for(int j=ids[0] < 0?1:0; j<cols; ++j)
+				{
+					prediction->data.fl[i + j*cols] *= maxNorm / sum;
+				}
+				sum = maxNorm;
+			}
+
+			// ADD virtual place prob
+			if(ids[0] < 0)
+			{
+				prediction->data.fl[i] = _predictionLC[0];
+				sum += prediction->data.fl[i];
 			}
 
 			//debug
 			//for(int j=0; j<cols; ++j)
 			//{
-			//	ULOGGER_DEBUG("test = %f", prediction->data.fl[i + j*cols]);
+			//	ULOGGER_DEBUG("test col=%d = %f", i, prediction->data.fl[i + j*cols]);
 			//}
+
+			if(sum<0.99 || sum > 1.01)
+			{
+				UWARN("Prediction is not normalized sum=%f", sum);
+			}
 		}
 		else
 		{
@@ -368,7 +381,6 @@ bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory,
 				}
 			}
 		}
-		++i;
 	}
 
 	ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
@@ -402,57 +414,21 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
 	_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
+float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
 {
-	if(!likelihoodIds.size() ||
-	   prediction == 0 ||
-	   prediction->rows != prediction->cols ||
-	   (unsigned int)prediction->rows != likelihoodIds.size() ||
-	   _predictionLC.size() < 2 ||
-	   !memory ||
-	   !prediction ||
-	   level<1)
+	if((unsigned int)prediction->cols != idToIndexMap.size() ||
+	   (unsigned int)prediction->rows != idToIndexMap.size())
 	{
-		ULOGGER_ERROR( "fail");
-		return 0;
+		UFATAL("Requirements no met");
 	}
 
-	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)
+	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
 	{
-		int index = uValue(likelihoodIds, iter->first, -1);
+		int index = uValue(idToIndexMap, 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);
+			sum += prediction->data.fl[col + index*prediction->cols] = _predictionLC[iter->second+1];
 		}
 	}
 	return sum;

corelib/src/BayesFilter.h

@@ -51,17 +51,19 @@ public:
 	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 isPredictionOnNonNullActionsOnly() const {return _predictionOnNonNullActionsOnly;}
 
-	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;
+	bool generatePrediction(CvMat * prediction, const Memory * memory, const std::vector<int> & ids) const;
 
 private:
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+	float addNeighborProb(CvMat * prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const;
 
 private:
 	std::map<int, float> _posterior;
 	float _virtualPlacePrior;
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+	bool _predictionOnNonNullActionsOnly;
 };
 
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -21,30 +21,109 @@ SET(SRC_FILES
 	KeypointDescriptor.cpp
 	VerifyHypotheses.cpp
 	NearestNeighbor.cpp
+	ColorTable.cpp
+
 )
 
 SET(INCLUDE_DIRS
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}
 	${CMAKE_CURRENT_BINARY_DIR}
-	${UTILITE_INCLUDE_DIR}
+	${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 	${SQLITE3_INCLUDE_DIR}
+	${ZLIB_INCLUDE_DIRS}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBS} 
 	${SQLITE3_LIBRARY}
+	${ZLIB_LIBRARIES}
 )
 
+####################################
 # 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]"
+   COMMENT "[Creating database resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
 )
 
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
+)
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
+   COMMENT "[Creating color table resource]"
+   DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
+)
+SET(RESOURCES 
+	${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h 
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
+	${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
+)
+
+####################################
+# Generate resources files END
+####################################
+
+
 # Make sure the compiler can find include files from our library.
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
@@ -58,7 +137,7 @@ 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)
+ADD_LIBRARY(corelib ${SRC_FILES} ${RESOURCES})
 TARGET_LINK_LIBRARIES(corelib ${LIBRARIES})
 
 SET_TARGET_PROPERTIES(

corelib/src/Camera.cpp

@@ -54,10 +54,10 @@ CamKeypointTreatment::~CamKeypointTreatment()
 }
 void CamKeypointTreatment::process(SMState * smState) const
 {
-	if(smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().size() == 0)
+	if(_keypointDetector && _keypointDescriptor && smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().empty())
 	{
-		std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
-		std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
+		std::vector<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
+		cv::Mat descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
 		smState->setSensors(descriptors);
 		smState->setKeypoints(keypoints);
 	}
@@ -90,6 +90,9 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
 		case kDetectorSift:
 			_keypointDetector = new SIFTDetector(parameters);
 			break;
+		case kDetectorFast:
+			_keypointDetector = new FASTDetector(parameters);
+			break;
 		case kDetectorSurf:
 		default:
 			_keypointDetector = new SURFDetector(parameters);
@@ -117,20 +120,17 @@ void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
 		}
 		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));
+		case kDescriptorBrief:
+			_keypointDescriptor = new BRIEFDescriptor(parameters);
+			break;
+		case kDescriptorColor:
+			_keypointDescriptor = new ColorDescriptor(parameters);
+			break;
+		case kDescriptorHue:
+			_keypointDescriptor = new HueDescriptor(parameters);
 			break;
 		case kDescriptorSurf:
 		default:
@@ -174,12 +174,25 @@ Camera::Camera(float imageRate,
 	UEventsManager::addHandler(this);
 }
 
-Camera::~Camera(void)
+Camera::~Camera()
 {
-	this->kill();
+	join(true);
 	delete _postThreatement;
 }
 
+SMState * Camera::takeSMState()
+{
+	std::list<std::vector<float> > actions;
+	IplImage * img = this->takeImage(&actions);
+	if(img)
+	{
+		SMState * smState = new SMState(cv::Mat(), actions);
+		smState->setImage(img);
+		return smState;
+	}
+	return 0;
+}
+
 void Camera::mainLoop()
 {
 	State state = kStateCapturing;
@@ -231,36 +244,6 @@ void Camera::pushNewState(State newState, const ParametersMap & parameters)
 
 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())
@@ -281,7 +264,7 @@ void Camera::process()
 {
 	UTimer timer;
 	ULOGGER_DEBUG("Camera::process()");
-	SMState * smState = this->takeImage();
+	SMState * smState = this->takeSMState();
 	if(smState)
 	{
 		_postThreatement->process(smState);
@@ -336,7 +319,7 @@ CameraImages::CameraImages(const std::string & path,
 
 CameraImages::~CameraImages(void)
 {
-	this->kill();
+	join(true);
 	if(_dir)
 	{
 		delete _dir;
@@ -361,11 +344,19 @@ bool CameraImages::init()
 	{
 		ULOGGER_ERROR("Directory path not valid \"%s\"", _path.c_str());
 	}
+	else if(_dir->getFileNames().size() == 0)
+	{
+		UWARN("Directory is empty \"%s\"", _path.c_str());
+	}
 	return _dir != 0;
 }
 
-SMState * CameraImages::takeImage()
+IplImage * CameraImages::takeImage(std::list<std::vector<float> > * actions)
 {
+	if(actions)
+	{
+		actions->clear();
+	}
 	IplImage * img = 0;
 	if(_dir)
 	{
@@ -406,6 +397,10 @@ SMState * CameraImages::takeImage()
 			}
 		}
 	}
+	else
+	{
+		UWARN("Directory is not set, camera must be initialized.");
+	}
 	if(img &&
 	   getImageWidth() &&
 	   getImageHeight() &&
@@ -422,11 +417,7 @@ SMState * CameraImages::takeImage()
 		cvReleaseImage(&img);
 		img = resampledImg;
 	}
-	if(img)
-	{
-		return new SMState(img);
-	}
-	return 0;
+	return img;
 }
 
 
@@ -461,7 +452,7 @@ CameraVideo::CameraVideo(const std::string & fileName,
 
 CameraVideo::~CameraVideo()
 {
-	this->kill();
+	join(true);
 	if(_capture)
 	{
 		cvReleaseCapture(&_capture);
@@ -503,8 +494,12 @@ bool CameraVideo::init()
 	return true;
 }
 
-SMState * CameraVideo::takeImage()
+IplImage * CameraVideo::takeImage(std::list<std::vector<float> > * actions)
 {
+	if(actions)
+	{
+		actions->clear();
+	}
 	IplImage * img = 0;  // Null image
 	if(_capture)
 	{
@@ -528,9 +523,9 @@ SMState * CameraVideo::takeImage()
 	   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 );
+		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);
@@ -541,11 +536,7 @@ SMState * CameraVideo::takeImage()
 		img = cvCloneImage(img);
 	}
 
-	if(img)
-	{
-		return new SMState(img);
-	}
-	return 0;
+	return img;
 }
 
 
@@ -558,14 +549,14 @@ SMState * CameraVideo::takeImage()
 // CameraDatabase
 /////////////////////////
 CameraDatabase::CameraDatabase(const std::string & path,
-								bool ignoreChildren,
+								bool loadActions,
 								float imageRate,
 								bool autoRestart,
 								unsigned int imageWidth,
 								unsigned int imageHeight) :
 	Camera(imageRate, autoRestart, imageWidth, imageHeight),
 	_path(path),
-	_ignoreChildren(ignoreChildren),
+	_loadActions(loadActions),
 	_indexIter(_ids.begin()),
 	_dbDriver(0)
 {
@@ -573,7 +564,7 @@ CameraDatabase::CameraDatabase(const std::string & path,
 
 CameraDatabase::~CameraDatabase(void)
 {
-	this->kill();
+	join(true);
 	if(_dbDriver)
 	{
 		_dbDriver->closeConnection();
@@ -614,37 +605,41 @@ bool CameraDatabase::init()
 	return true;
 }
 
-SMState * CameraDatabase::takeImage()
+IplImage * CameraDatabase::takeImage(std::list<std::vector<float> > * actions)
 {
+	if(actions)
+	{
+		actions->clear();
+	}
 	IplImage * img = 0;
 	if(_dbDriver && _indexIter != _ids.end())
 	{
-		if(_ignoreChildren)
+		// Get image
+		_dbDriver->getImage(*_indexIter, &img);
+
+		// Get actions from its previous neighbor
+		if(actions && _loadActions)
 		{
-			bool ignore = true;
-			while(img == 0 && _indexIter != _ids.end() && ignore)
+			if(*_indexIter-1 > 0)
 			{
-				ignore = false;
-				int loopId = 0;
-				if(_dbDriver->getLoopClosureId(*_indexIter, loopId))
+				NeighborsMultiMap neighbors;
+				_dbDriver->loadNeighbors(*_indexIter-1, neighbors);
+				for(NeighborsMultiMap::iterator iter = neighbors.begin(); iter!=neighbors.end(); ++iter)
 				{
-					if(loopId == *_indexIter+1)
+					if(iter->first>*_indexIter-1 && iter->second.actions().size())
 					{
-						ignore = true;
-					}
-					else
-					{
-						_dbDriver->getImage(*_indexIter, &img);
+						*actions = iter->second.actions();
+						break;
 					}
 				}
-				++_indexIter;
+				if(actions->size() == 0)
+				{
+					UWARN("actions from previous %d to current %d are null", *_indexIter-1, *_indexIter);
+				}
 			}
 		}
-		else
-		{
-			_dbDriver->getImage(*_indexIter, &img);
-			++_indexIter;
-		}
+
+		++_indexIter;
 	}
 	else if(!_dbDriver)
 	{
@@ -672,27 +667,7 @@ SMState * CameraDatabase::takeImage()
 		img = resampledImg;
 	}
 
-	if(img)
-	{
-		SMState * smState = new SMState(img);
-		if(_dbDriver && _indexIter!=_ids.begin())
-		{
-			std::set<int>::iterator iter = _indexIter;
-			--iter;
-			std::map<int, std::list<std::vector<float> > > neighbors;
-			_dbDriver->loadNeighbors(*iter, neighbors);
-			for(std::map<int, std::list<std::vector<float> > >::iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
-			{
-				if(i->first > *iter && i->second.size())
-				{
-					smState->setActuators(i->second);
-					break;
-				}
-			}
-		}
-		return smState;
-	}
-	return 0;
+	return img;
 }
 
 } // namespace rtabmap

corelib/src/ColorTable.h

@@ -0,0 +1,40 @@
+
+#ifndef COLORTABLE_H
+#define COLORTABLE_H
+
+#include <vector>
+
+namespace rtabmap
+{
+class ColorTable
+{
+public:
+	ColorTable(int size);
+	virtual ~ColorTable() {}
+
+	static unsigned char INDEXED_TABLE_8[24];
+	static unsigned char INDEXED_TABLE_16[48];
+	static unsigned char INDEXED_TABLE_32[96];
+	static unsigned char INDEXED_TABLE_64[192];
+	static unsigned char INDEXED_TABLE_128[384];
+	static unsigned char INDEXED_TABLE_256[768];
+	static unsigned char INDEXED_TABLE_512[1536];
+	static unsigned char INDEXED_TABLE_1024[3076];
+	static unsigned char INDEXED_TABLE_65536[196608];
+
+	int size() const {return _size;}
+	unsigned short getIndex(unsigned char r, unsigned char g, unsigned char b) const;
+	void getRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
+
+	unsigned short getNNIndex(unsigned char r, unsigned char g, unsigned char b) const;
+	void getNNRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
+
+private:
+	int _size;
+	std::vector<unsigned short> _rgb2indexed;
+	unsigned char * _indexedTable;
+};
+
+} // namespace rtabmap
+
+#endif // COLORTABLE_H

corelib/src/DBDriver.cpp

@@ -19,7 +19,7 @@
 
 #include "rtabmap/core/DBDriver.h"
 
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "VWDictionary.h"
 #include "utilite/UConversion.h"
 #include "utilite/UMath.h"
@@ -32,6 +32,7 @@ namespace rtabmap {
 DBDriver::DBDriver(const ParametersMap & parameters) :
 	_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
 	_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
+	_imagesCompressed(Parameters::defaultDbImagesCompressed()),
 	_asyncWaiting(true),
 	_emptyTrashesTime(0)
 {
@@ -40,7 +41,7 @@ DBDriver::DBDriver(const ParametersMap & parameters) :
 
 DBDriver::~DBDriver()
 {
-	this->kill();
+	join(true);
 	this->emptyTrashes();
 }
 
@@ -55,24 +56,31 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
 	{
 		_minWordsToSave = std::atoi((*iter).second.c_str());
 	}
+	if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
+	{
+		_imagesCompressed = uStr2Bool((*iter).second.c_str());
+	}
 }
 
 void DBDriver::closeConnection()
 {
-	this->kill();
+	UDEBUG("isRunning=%d", this->isRunning());
+	this->join(true);
+	UDEBUG("");
 	this->emptyTrashes();
 	_dbSafeAccessMutex.lock();
 	this->disconnectDatabaseQuery();
 	_dbSafeAccessMutex.unlock();
+	UDEBUG("");
 }
 
-bool DBDriver::openConnection(const std::string & url)
+bool DBDriver::openConnection(const std::string & url, bool overwritten)
 {
+	UDEBUG("");
 	_url = url;
 	_dbSafeAccessMutex.lock();
-	if(this->connectDatabaseQuery(url))
+	if(this->connectDatabaseQuery(url, overwritten))
 	{
-		this->start();
 		_dbSafeAccessMutex.unlock();
 		return true;
 	}
@@ -103,6 +111,7 @@ void DBDriver::mainLoop()
 {
 	UDEBUG("");
 	this->emptyTrashes();
+	UDEBUG("");
 	this->kill(); // Do it only once
 	UDEBUG("");
 }
@@ -243,19 +252,15 @@ 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(_trashSignatures.size())
 		{
-			if(i->first == signatureId)
+			_dbSafeAccessMutex.lock();
+			_dbSafeAccessMutex.unlock();
+			std::map<int, Signature*>::iterator iter =_trashSignatures.find(signatureId);
+			if(iter != _trashSignatures.end())
 			{
-				*s = i->second;
-				_trashSignatures.erase(i++);
-				break;
-			}
-			else
-			{
-				++i;
+				*s = iter->second;
+				_trashSignatures.erase(iter);
 			}
 		}
 	}
@@ -278,15 +283,15 @@ 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(_trashVisualWords.size())
 		{
-			if((*i).first == wordId)
+			_dbSafeAccessMutex.lock();
+			_dbSafeAccessMutex.unlock();
+			std::map<int, VisualWord*>::iterator iter = _trashVisualWords.find(wordId);
+			if(iter != _trashVisualWords.end())
 			{
-				*vw = (*i).second;
-				_trashVisualWords.erase(i);
-				break;
+				*vw = iter->second;
+				_trashVisualWords.erase(iter);
 			}
 		}
 	}
@@ -308,7 +313,7 @@ bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
 bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 {
 	ULOGGER_DEBUG("");
-	std::list<KeypointSignature *> toSaveK;
+	std::list<Signature *> toSave;
 	std::list<Signature *> toUpdate;
 	if(this->isConnected() && signatures.size())
 	{
@@ -318,13 +323,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 			{
 				toUpdate.push_back(*i);
 			}
-			else if((*i)->signatureType().compare("KeypointSignature") == 0)
-			{
-				toSaveK.push_back((KeypointSignature *)(*i));
-			}
 			else
 			{
-				ULOGGER_ERROR("Unknown signature type ?!?");
+				toSave.push_back(*i);
 			}
 		}
 
@@ -332,9 +333,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 		{
 			this->updateQuery(toUpdate);
 		}
-		if(toSaveK.size())
+		if(toSave.size())
 		{
-			this->saveQuery(toSaveK);
+			this->saveQuery(toSave);
 		}
 	}
 	return false;
@@ -358,7 +359,7 @@ bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
 	return r;
 }
 
-bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, bool onlyParents)
+bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
 {
 	UDEBUG("");
 	// look up in the trash before the database
@@ -376,15 +377,9 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
 			{
 				if(sIter->first == *iter)
 				{
-					if((onlyParents && sIter->second->getLoopClosureId() == 0) || !onlyParents)
-					{
-						signatures.push_back(sIter->second);
-						_trashSignatures.erase(sIter++);
-					}
-					else
-					{
-						++sIter;
-					}
+					signatures.push_back(sIter->second);
+					_trashSignatures.erase(sIter++);
+
 					valueFound = true;
 					break;
 				}
@@ -409,7 +404,64 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
 	{
 		bool r;
 		_dbSafeAccessMutex.lock();
-		r = this->loadKeypointSignaturesQuery(ids, signatures, onlyParents);
+		r = this->loadKeypointSignaturesQuery(ids, signatures);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	else if(signatures.size())
+	{
+		return true;
+	}
+	return false;
+}
+
+// TODO the same code of method loadKeypointSignatures() above is used here
+bool DBDriver::loadSMSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
+{
+	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)
+				{
+					signatures.push_back(sIter->second);
+					_trashSignatures.erase(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->loadSMSignaturesQuery(ids, signatures);
 		_dbSafeAccessMutex.unlock();
 		return r;
 	}
@@ -429,23 +481,27 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
 	// look up in the trash before the database
 	std::list<int> ids = wordIds;
 	std::map<int, VisualWord*>::iterator wIter;
+	std::list<VisualWord *> puttedBack;
 	_trashesMutex.lock();
 	{
-		_dbSafeAccessMutex.lock();
-		_dbSafeAccessMutex.unlock();
-		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
+		if(_trashVisualWords.size())
 		{
-			wIter = _trashVisualWords.find(*iter);
-			if(wIter != _trashVisualWords.end())
+			_dbSafeAccessMutex.lock();
+			_dbSafeAccessMutex.unlock();
+			for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
 			{
-				//UDEBUG("put back word %d from trash", *iter);
-				vws.push_back(wIter->second);
-				_trashVisualWords.erase(wIter);
-				iter = ids.erase(iter);
-			}
-			else
-			{
-				++iter;
+				wIter = _trashVisualWords.find(*iter);
+				if(wIter != _trashVisualWords.end())
+				{
+					UDEBUG("put back word %d from trash", *iter);
+					puttedBack.push_back(wIter->second);
+					_trashVisualWords.erase(wIter);
+					iter = ids.erase(iter);
+				}
+				else
+				{
+					++iter;
+				}
 			}
 		}
 	}
@@ -456,10 +512,12 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
 		_dbSafeAccessMutex.lock();
 		r = this->loadWordsQuery(ids, vws);
 		_dbSafeAccessMutex.unlock();
+		uAppend(vws, puttedBack);
 		return r;
 	}
-	else if(vws.size())
+	else if(puttedBack.size())
 	{
+		uAppend(vws, puttedBack);
 		return true;
 	}
 	return false;
@@ -571,78 +629,27 @@ bool DBDriver::deleteUnreferencedWords() const
 	return false;
 }
 
-bool DBDriver::addNeighbor(int id, int newNeighbor, int oldNeighbor)
-{
-	bool r = false;
-	Signature * s = 0;
-	_trashesMutex.lock();
-	s = uValue(_trashSignatures, id, s);
-	if(s)
-	{
-		std::list<std::vector<float> > actions = uValue(s->getNeighbors(), oldNeighbor, std::list<std::vector<float> >());
-		s->addNeighbor(newNeighbor, actions);
-		r = true;
-	}
-	_trashesMutex.unlock();
-
-	if(!r)
-	{
-		_dbSafeAccessMutex.lock();
-		r = this->addNeighborQuery(id, newNeighbor, oldNeighbor);
-		_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);
-	}
+	bool result = this->getImageQuery(id, img);
 	_dbSafeAccessMutex.unlock();
 	return result;
 }
 
 //TODO Check also in the trash ?
-bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors) const
+bool DBDriver::getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions) const
 {
 	bool r;
 	_dbSafeAccessMutex.lock();
-	r = this->getNeighborIdsQuery(signatureId, neighbors);
+	r = this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
 	_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 DBDriver::loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const
 {
 	bool r;
 	_dbSafeAccessMutex.lock();
@@ -662,21 +669,11 @@ bool DBDriver::getWeight(int signatureId, int & weight) const
 }
 
 //TODO Check also in the trash ?
-bool DBDriver::getLoopClosureId(int signatureId, int & loopId) const
+bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) 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);
+	r = this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
 	_dbSafeAccessMutex.unlock();
 	return r;
 }
@@ -721,16 +718,6 @@ bool DBDriver::getSurfNi(int signatureId, int & ni) const
 	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;

corelib/src/DBDriverSqlite3.h

@@ -32,13 +32,16 @@ public:
 	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
 	virtual ~DBDriverSqlite3();
 
+	virtual std::string getDriverName() const {return "sqlite3";}
 	virtual void parseParameters(const ParametersMap & parameters);
 	void setDbInMemory(bool dbInMemory);
 	void setJournalMode(int journalMode);
 	void setCacheSize(unsigned int cacheSize);
+	void setSynchronous(int synchronous);
+	void setTempStore(int tempStore);
 
 private:
-	virtual bool connectDatabaseQuery(const std::string & url);
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwirtten = false);
 	virtual void disconnectDatabaseQuery();
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
@@ -47,15 +50,13 @@ private:
 
 	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 getNeighborIdsQuery(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const;
 	virtual bool getWeightQuery(int signatureId, int & weight) const;
-	virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const;
-	virtual bool addNeighborQuery(int id, int newNeighbor, int oldNeighbor) const;
+	virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) 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;
+	virtual bool saveQuery(const std::list<Signature *> & signatures) const;
 
 	// Load objects
 	virtual bool loadQuery(VWDictionary * dictionary) const;
@@ -63,18 +64,34 @@ private:
 	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 loadQuery(int signatureId, SMSignature * ss) const;
+	virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
+	virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) 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 loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const;
+	bool loadNeighborsQuery(std::list<Signature *> & signatures) const;
 
-	virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const;
+	virtual bool getImageQuery(int id, IplImage ** image) 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:
+	std::string queryStepSignature() const;
+	std::string queryStepImage() const;
+	std::string queryStepNeighborLink() const;
+	std::string queryStepWordsChanged() const;
+	std::string queryStepKeypoint() const;
+	std::string queryStepSensors() const;
+	int stepSignature(sqlite3_stmt * ppStmt, const Signature * s) const;
+	int stepImage(sqlite3_stmt * ppStmt, int id, const IplImage * img) const;
+	int stepNeighborLink(sqlite3_stmt * ppStmt, int signatureId, const NeighborLink & n) const;
+	int stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
+	int stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
+	int stepSensors(sqlite3_stmt * ppStmt, const SMSignature * s) const;
+
 private:
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
@@ -83,6 +100,8 @@ private:
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;
+	int _synchronous;
+	int _tempStore;
 };
 
 }

corelib/src/KeypointDescriptor.cpp

@@ -31,72 +31,31 @@
 
 namespace rtabmap {
 
-KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
-	_childDescriptor(childDescriptor)
+KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
 {
 	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)
+SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
+	KeypointDescriptor(parameters)
 {
-	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
-	_surf.extended = Parameters::defaultSURFExtended();
-	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
-	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_params.extended = Parameters::defaultSURFExtended();
+	_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_params.nOctaves = Parameters::defaultSURFOctaves();
+	_params.upright = Parameters::defaultSURFUpright();
 	_gpuVersion = Parameters::defaultSURFGpuVersion();
-	_upright = Parameters::defaultSURFUpright();
 	this->parseParameters(parameters);
 }
 
@@ -109,35 +68,35 @@ 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());
+		_params.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?
+		_params.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?
+		_params.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?
+		_params.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
+	{
+		_params.upright = uStr2Bool((*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());
-	}
 	KeypointDescriptor::parseParameters(parameters);
 }
 
-std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+cv::Mat SURFDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	ULOGGER_DEBUG("");
-	std::list<std::vector<float> > descriptors;
+	cv::Mat descriptors;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -159,33 +118,35 @@ std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplIma
 	{
 		img =  cv::Mat(image);
 	}
-	cv::Mat mask;
-	std::vector<cv::KeyPoint> k = uListToVector(keypoints);
-	std::vector<float> d;
 #if OPENCV_SURF_GPU
 	if(_gpuVersion)
 	{
+		std::vector<float> d;
 		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);
+		cv::gpu::SURF_GPU surfGpu(_params.hessianThreshold, _params.nOctaves, _params.nOctaveLayers, _params.extended, 0.01f, _params.upright);
+		surfGpu.uploadKeypoints(keypoints, keypointsGpu);
 		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
 		surfGpu.downloadDescriptors(descriptorsGpu, d);
+		unsigned int dim = _params.extended?128:64;
+		descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
+		for(int i=0; i<descriptors.rows; ++i)
+		{
+			float * rowFl = descriptors.ptr<float>(i);
+			memcpy(rowFl, &d[i*dim], dim*sizeof(float));
+		}
 	}
 	else
 	{
-		_surf(img, mask, k, d, true); // Opencv surf descriptors
+		cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
+		extractor.compute(img, keypoints, descriptors);
 	}
 #else
-	_surf(img, mask, k, d, true); // Opencv surf descriptors
+	cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
+	extractor.compute(img, keypoints, 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);
@@ -196,8 +157,8 @@ std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplIma
 //////////////////////////
 //SIFTDescriptor
 //////////////////////////
-SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
-	KeypointDescriptor(parameters, childDescriptor)
+SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
+	KeypointDescriptor(parameters)
 {
 	this->parseParameters(parameters);
 }
@@ -212,10 +173,10 @@ void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
 	KeypointDescriptor::parseParameters(parameters);
 }
 
-std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+cv::Mat SIFTDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	ULOGGER_DEBUG("");
-	std::list<std::vector<float> > descriptors;
+	cv::Mat descriptors;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -237,17 +198,8 @@ std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplIma
 	{
 		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));
-	}
+	cv::SiftDescriptorExtractor extractor(_descriptorParams, _commonParams);
+	extractor.compute(img, keypoints, descriptors);
 	if(imageGrayScale)
 	{
 		cvReleaseImage(&imageGrayScale);
@@ -256,35 +208,60 @@ std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplIma
 }
 
 //////////////////////////
-//LaplacianDescriptor
+//BRIEFDescriptor
 //////////////////////////
-LaplacianDescriptor::LaplacianDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
-	KeypointDescriptor(parameters, childDescriptor)
+BRIEFDescriptor::BRIEFDescriptor(const ParametersMap & parameters) :
+	KeypointDescriptor(parameters),
+	_size(Parameters::defaultBRIEFSize())
 {
 	this->parseParameters(parameters);
 }
 
-LaplacianDescriptor::~LaplacianDescriptor()
+BRIEFDescriptor::~BRIEFDescriptor()
 {
 }
 
-void LaplacianDescriptor::parseParameters(const ParametersMap & parameters)
+void BRIEFDescriptor::parseParameters(const ParametersMap & parameters)
 {
-	// No parameter...
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kBRIEFSize())) != parameters.end())
+	{
+		_size = std::atoi((*iter).second.c_str());
+	}
 	KeypointDescriptor::parseParameters(parameters);
 }
 
-std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+cv::Mat BRIEFDescriptor::generateDescriptors(const IplImage * image, std::vector<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)
+	cv::Mat descriptors;
+	if(!image)
 	{
-		std::vector<float> laplacian(1);
-		laplacian[0] = uSign(key->response);
-		descriptors.push_back(laplacian);
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	// BRIEF 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::BriefDescriptorExtractor brief(_size);
+	brief.compute(img, keypoints, descriptors);
+
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
 	}
 	return descriptors;
 }
@@ -292,8 +269,8 @@ std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const I
 //////////////////////////
 //ColorDescriptor
 //////////////////////////
-ColorDescriptor::ColorDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
-	KeypointDescriptor(parameters, childDescriptor)
+ColorDescriptor::ColorDescriptor(const ParametersMap & parameters) :
+	KeypointDescriptor(parameters)
 {
 	this->parseParameters(parameters);
 }
@@ -308,10 +285,10 @@ void ColorDescriptor::parseParameters(const ParametersMap & parameters)
 	KeypointDescriptor::parseParameters(parameters);
 }
 
-std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+cv::Mat ColorDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	ULOGGER_DEBUG("");
-	std::list<std::vector<float> > descriptors;
+	cv::Mat descriptors;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -335,7 +312,9 @@ std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplIm
 	}
 
 	//create descriptors...
-	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	descriptors = cv::Mat(keypoints.size(), 6, CV_32F);
+	int i=0;
+	for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
 	{
 
 		int grayMax = -1;  // grayValue
@@ -380,11 +359,11 @@ std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplIm
 				}
 			}
 		}
-		for(int i=0; i<6; ++i)
+		for(int j=0; j<6; ++j)
 		{
-			d[i] /= 255; // Normalize between 0 and 1
+			descriptors.at<float>(i,j) = d[j] / 255; // Normalize between 0 and 1
 		}
-		descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
+		++i;
 	}
 
 	if(imageConverted)
@@ -409,8 +388,8 @@ void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
 //////////////////////////
 //HueDescriptor
 //////////////////////////
-HueDescriptor::HueDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
-		ColorDescriptor(parameters, childDescriptor)
+HueDescriptor::HueDescriptor(const ParametersMap & parameters) :
+		ColorDescriptor(parameters)
 {
 	this->parseParameters(parameters);
 }
@@ -425,10 +404,10 @@ void HueDescriptor::parseParameters(const ParametersMap & parameters)
 	KeypointDescriptor::parseParameters(parameters);
 }
 
-std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+cv::Mat HueDescriptor::generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	ULOGGER_DEBUG("");
-	std::list<std::vector<float> > descriptors;
+	cv::Mat descriptors;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -452,7 +431,9 @@ std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImag
 	}
 
 	//create descriptors...
-	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	descriptors = cv::Mat(keypoints.size(), 2, CV_32F);
+	int i=0;
+	for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
 	{
 
 		int intensityMax = -1;
@@ -512,7 +493,9 @@ std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImag
 		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)));
+		float * rowFl = descriptors.ptr<float>(i);
+		memcpy(rowFl, &d[i*2], 2*sizeof(float));
+		++i;
 	}
 
 	if(imageConverted)

corelib/src/KeypointDetector.cpp

@@ -60,10 +60,10 @@ void KeypointDetector::parseParameters(const ParametersMap & parameters)
 	}
 }
 
-std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
+std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
 {
 	ULOGGER_DEBUG("");
-	std::list<cv::KeyPoint> keypoints;
+	std::vector<cv::KeyPoint> keypoints;
 	if(image)
 	{
 		UTimer timer;
@@ -97,28 +97,40 @@ std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * ima
 					// 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)
+					std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
+					for(std::vector<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));
+						hessianMap.insert(std::pair<float, std::vector<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)
+					int removed = hessianMap.size()-_wordsPerImageTarget;
+					std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
+					std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
+					for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
 					{
-						keypoints.erase(iter->second);
-						++removed;
+						kptsTmp[k] = *iter->second;
+						// Adjust keypoint position to raw image
+						kptsTmp[k].pt.x += roi.x;
+						kptsTmp[k].pt.y += roi.y;
 					}
 					if(iter->first!=0)
 					{
 						_adaptiveResponseThr = iter->first;
 					}
+					keypoints = kptsTmp;
 					ULOGGER_DEBUG("%d keypoints removed, (kept %d)", removed, keypoints.size());
 				}
+				else if(roi.x || roi.y)
+				{
+					// Adjust keypoint position to raw image
+					for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+					{
+						iter->pt.x += roi.x;
+						iter->pt.y += roi.y;
+					}
+				}
 			}
 			else
 			{
@@ -133,12 +145,14 @@ std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * ima
 			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)
+		else if(roi.x || roi.y)
 		{
-			iter->pt.x += roi.x;
-			iter->pt.y += roi.y;
+			// Adjust keypoint position to raw image
+			for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+			{
+				iter->pt.x += roi.x;
+				iter->pt.y += roi.y;
+			}
 		}
 	}
 	else
@@ -232,14 +246,14 @@ cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
 SURFDetector::SURFDetector(const ParametersMap & parameters) :
 		KeypointDetector(parameters)
 {
-	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
-	_surf.extended = Parameters::defaultSURFExtended();
-	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
-	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_params.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_params.extended = Parameters::defaultSURFExtended();
+	_params.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_params.nOctaves = Parameters::defaultSURFOctaves();
 	_gpuVersion = Parameters::defaultSURFGpuVersion();
-	_upright = Parameters::defaultSURFUpright();
+	_params.upright = Parameters::defaultSURFUpright();
 	this->parseParameters(parameters);
-	this->setAdaptiveResponseThr(_surf.hessianThreshold);
+	this->setAdaptiveResponseThr(_params.hessianThreshold);
 }
 
 SURFDetector::~SURFDetector()
@@ -251,24 +265,24 @@ 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());
+		_params.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);
+		_params.hessianThreshold = std::atof((*iter).second.c_str());
+		this->setAdaptiveResponseThr(_params.hessianThreshold);
 	}
 	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
 	{
-		_surf.nOctaveLayers = std::atoi((*iter).second.c_str());
+		_params.nOctaveLayers = std::atoi((*iter).second.c_str());
 	}
 	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
 	{
-		_surf.nOctaves = std::atoi((*iter).second.c_str());
+		_params.nOctaves = std::atoi((*iter).second.c_str());
 	}
 	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
 	{
-		_surf.nOctaves = std::atoi((*iter).second.c_str());
+		_params.nOctaves = std::atoi((*iter).second.c_str());
 	}
 	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
 	{
@@ -276,15 +290,15 @@ void SURFDetector::parseParameters(const ParametersMap & parameters)
 	}
 	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
 	{
-		_upright = uStr2Bool((*iter).second.c_str());
+		_params.upright = uStr2Bool((*iter).second.c_str());
 	}
 	KeypointDetector::parseParameters(parameters);
 }
 
-std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
-	std::list<cv::KeyPoint> keypoints;
+	std::vector<cv::KeyPoint> keypoints;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -307,32 +321,31 @@ std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image,
 		img =  cv::Mat(image);
 	}
 
-	cv::SURF surf = _surf;
+	CvSURFParams params = _params;
 	if(this->isUsingAdaptiveResponseThr())
 	{
-		surf.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+		params.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);
+		cv::gpu::SURF_GPU surfGpu(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.extended, 0.01f, params.upright);
 		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
-		surfGpu.downloadKeypoints(keypointsGpu, k);
+		surfGpu.downloadKeypoints(keypointsGpu, keypoints);
 	}
 	else
 	{
-		surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+		cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
+		detector.detect(imgRoi, keypoints);
 	}
 #else
-	surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+	cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
+	detector.detect(imgRoi, keypoints);
 #endif
-	keypoints = uVectorToList(k);
-
 
 	if(imageGrayScale)
 	{
@@ -372,10 +385,10 @@ void SIFTDetector::parseParameters(const ParametersMap & parameters)
 	KeypointDetector::parseParameters(parameters);
 }
 
-std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
-	std::list<cv::KeyPoint> keypoints;
+	std::vector<cv::KeyPoint> keypoints;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -403,13 +416,10 @@ std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image,
 	{
 		detectorParam.threshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
 	}
-	cv::Mat mask;
-	cv::SIFT sift(_commonParams, detectorParam);
 
+	cv::SiftFeatureDetector detector(detectorParam, _commonParams);
 	cv::Mat imgRoi(img, roi);
-	std::vector<cv::KeyPoint> k;
-	sift(imgRoi, mask, k); // Opencv surf keypoints
-	keypoints = uVectorToList(k);
+	detector.detect(imgRoi, keypoints); // Opencv surf keypoints
 	if(imageGrayScale)
 	{
 		cvReleaseImage(&imageGrayScale);
@@ -424,13 +434,13 @@ std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image,
 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();
+	_params.lineThresholdBinarized = Parameters::defaultStarLineThresholdBinarized();
+	_params.lineThresholdProjected = Parameters::defaultStarLineThresholdProjected();
+	_params.maxSize = Parameters::defaultStarMaxSize();
+	_params.responseThreshold = Parameters::defaultStarResponseThreshold();
+	_params.suppressNonmaxSize = Parameters::defaultStarSuppressNonmaxSize();
 	this->parseParameters(parameters);
-	this->setAdaptiveResponseThr(_star.responseThreshold);
+	this->setAdaptiveResponseThr(_params.responseThreshold);
 }
 
 StarDetector::~StarDetector()
@@ -443,32 +453,32 @@ void StarDetector::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
 	{
-		_star.lineThresholdBinarized = std::atoi((*iter).second.c_str());
+		_params.lineThresholdBinarized = std::atoi((*iter).second.c_str());
 	}
 	if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
 	{
-		_star.lineThresholdProjected = std::atoi((*iter).second.c_str());
+		_params.lineThresholdProjected = std::atoi((*iter).second.c_str());
 	}
 	if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
 	{
-		_star.maxSize = std::atoi((*iter).second.c_str());
+		_params.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);
+		_params.responseThreshold = int(std::atof((*iter).second.c_str()));
+		this->setAdaptiveResponseThr(_params.responseThreshold);
 	}
 	if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
 	{
-		_star.suppressNonmaxSize = std::atoi((*iter).second.c_str());
+		_params.suppressNonmaxSize = std::atoi((*iter).second.c_str());
 	}
 	KeypointDetector::parseParameters(parameters);
 }
 
-std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
-	std::list<cv::KeyPoint> keypoints;
+	std::vector<cv::KeyPoint> keypoints;
 	if(!image)
 	{
 		ULOGGER_ERROR("Image is null ?!?");
@@ -476,20 +486,72 @@ std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image,
 	}
 
 	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;
+	CvStarDetectorParams params = _params;
 	if(this->isUsingAdaptiveResponseThr())
 	{
-		star.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+		params.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);
+	cv::StarFeatureDetector detector(params);
+	detector.detect(imgRoi, keypoints);
+	return keypoints;
+}
+
+//////////////////////////
+//FastDetector
+//////////////////////////
+FASTDetector::FASTDetector(const ParametersMap & parameters) :
+	KeypointDetector(parameters),
+	_threshold(Parameters::defaultFASTThreshold()),
+	_nonmaxSuppression(Parameters::defaultFASTNonmaxSuppression())
+{
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_threshold);
+}
+
+FASTDetector::~FASTDetector()
+{
+}
+
+void FASTDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kFASTThreshold())) != parameters.end())
+	{
+		_threshold = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kFASTNonmaxSuppression())) != parameters.end())
+	{
+		_nonmaxSuppression = uStr2Bool((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::vector<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+
+	cv::Mat img(image);
+	cv::Mat imgRoi(img, roi);
+	int threshold = _threshold;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		threshold = (int)this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+	cv::FastFeatureDetector fast(threshold, _nonmaxSuppression);
+
+	// Get keypoints with the fast detector
+	fast.detect(imgRoi, keypoints);
 	return keypoints;
 }
 

corelib/src/KeypointMemory.cpp

@@ -19,7 +19,7 @@
 
 #include "KeypointMemory.h"
 #include "VWDictionary.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/DBDriver.h"
 #include "utilite/UtiLite.h"
 #include "rtabmap/core/Parameters.h"
@@ -43,7 +43,6 @@ KeypointMemory::KeypointMemory(const ParametersMap & parameters) :
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
 	_parallelized(Parameters::defaultKpParallelized()),
-	_sensorStateOnly(Parameters::defaultKpSensorStateOnly()),
 	_tfIdfNormalized(Parameters::defaultKpTfIdfNormalized())
 {
 	_vwd = new VWDictionary(parameters);
@@ -95,11 +94,6 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
 		_parallelized = uStr2Bool((*iter).second.c_str());
 	}
 
-	if((iter=parameters.find(Parameters::kKpSensorStateOnly())) != parameters.end())
-	{
-		_sensorStateOnly = uStr2Bool((*iter).second.c_str());
-	}
-
 	if((iter=parameters.find(Parameters::kKpTfIdfNormalized())) != parameters.end())
 	{
 		_tfIdfNormalized = uStr2Bool((*iter).second.c_str());
@@ -133,6 +127,9 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
 		case kDetectorSift:
 			_keypointDetector = new SIFTDetector(parameters);
 			break;
+		case kDetectorFast:
+			_keypointDetector = new FASTDetector(parameters);
+			break;
 		case kDetectorSurf:
 		default:
 			_keypointDetector = new SURFDetector(parameters);
@@ -160,20 +157,17 @@ void KeypointMemory::parseParameters(const ParametersMap & parameters)
 		}
 		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));
+		case kDescriptorBrief:
+			_keypointDescriptor = new BRIEFDescriptor(parameters);
+			break;
+		case kDescriptorColor:
+			_keypointDescriptor = new ColorDescriptor(parameters);
+			break;
+		case kDescriptorHue:
+			_keypointDescriptor = new HueDescriptor(parameters);
 			break;
 		case kDescriptorSurf:
 		default:
@@ -207,7 +201,7 @@ KeypointMemory::DetectorStrategy KeypointMemory::detectorStrategy() const
 
 bool KeypointMemory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
 {
-	ULOGGER_DEBUG("KeypointMemory::init()");
+	UDEBUG("");
 	// This will open a connection to the database,
 	// this calls also clear()
 	bool success = Memory::init(dbDriverName, dbUrl, dbOverwritten, parameters);
@@ -217,8 +211,8 @@ bool KeypointMemory::init(const std::string & dbDriverName, const std::string &
 	{
 		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary...")));
 		_dbDriver->load(_vwd);
-		ULOGGER_DEBUG("%d words loaded!", _vwd->getVisualWords().size());
-		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2str(int(_vwd->getVisualWords().size())) + " loaded)"));
+		UDEBUG("%d words loaded!", _vwd->getVisualWords().size());
+		UEventsManager::post(new RtabmapEventInit(std::string("Loading dictionary, done! (") + uNumber2Str(int(_vwd->getVisualWords().size())) + " loaded)"));
 	}
 
 	// Enable loaded signatures
@@ -290,7 +284,7 @@ void KeypointMemory::clear()
 
 	if(_dbDriver)
 	{
-		_dbDriver->kill();
+		_dbDriver->join(true);
 		cleanUnusedWords();
 		_dbDriver->emptyTrashes();
 
@@ -302,7 +296,7 @@ void KeypointMemory::clear()
 			// all signatures with the old word to the active one...
 			//_dbDriver->changeWordsRef(_wordRefsToChange);
 			//remove old values
-			_dbDriver->deleteUnreferencedWords();
+			//_dbDriver->deleteUnreferencedWords();
 		//	_dbDriver->commit();
 		//}
 		ULOGGER_DEBUG("");
@@ -328,40 +322,6 @@ void KeypointMemory::preUpdate()
 	}
 }
 
-
-// TODO Really useful?
-/*void KeypointMemory::postUpdate()
-{
-	ULOGGER_DEBUG("");
-	// Detect if the last signature is a bad one. If the signature has less than 15% of
-	// the average words/signature.
-	KeypointSignature * ss = dynamic_cast<KeypointSignature *>(this->_getLastSignature());
-
-	float ratio = 0;
-	if(ss)
-	{
-		ratio = float(uUniqueKeys(ss->getWords()).size()) / float(ss->getWords().size());
-	}
-
-	int nbCommonWords = 0;
-	ULOGGER_DEBUG("_workingMem.size() = %d, _stMem.size()=%d", _workingMem.size(), _stMem.size());
-	int treeSize= _workingMem.size() + _stMem.size();//Don't count the virtual place
-	if(treeSize > 0)
-	{
-		nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
-	}
-
-	ULOGGER_DEBUG("ratio=%f, treeSize=%d, nbCommonWords=%d", ratio, treeSize, nbCommonWords);
-
-	if(//(ratio < _badSignRatio) ||
-	   (nbCommonWords && ss && ss->getWords().size() < _badSignRatio * nbCommonWords))
-	{
-		ULOGGER_WARN("id %d is a bad signature", ss->id());
-		this->disableWordsRef(ss->id());
-		ss->removeAllWords();
-	}
-}*/
-
 // NON class method! only used in merge()
 std::multimap<int, cv::KeyPoint> getMostDescriptiveWords(const std::multimap<int, cv::KeyPoint> & words, int max, const std::set<int> & ignoredIds)
 {
@@ -398,49 +358,57 @@ std::multimap<int, cv::KeyPoint> getMostDescriptiveWords(const std::multimap<int
 	return mostDescriptiveWords;
 }
 
-void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrategy s)
+std::multimap<int, cv::KeyPoint> KeypointMemory::getWords(int signatureId) const
 {
-	// The signatures must be KeypointSignature
-	const KeypointSignature * sFrom = dynamic_cast<const KeypointSignature *>(from);
-	KeypointSignature * sTo = dynamic_cast<KeypointSignature *>(to);
-	UTimer timer;
-	timer.start();
-	if(sFrom && sTo)
+	std::multimap<int, cv::KeyPoint> words;
+	if(signatureId>0)
 	{
-		if(s == kUseOnlyFromMerging)
+		const Signature * s = this->getSignature(signatureId);
+		if(s)
 		{
-			this->disableWordsRef(sTo->id());
-			sTo->setWords(sFrom->getWords());
+			const KeypointSignature * ks = dynamic_cast<const KeypointSignature*>(s);
+			if(ks)
+			{
+				words = ks->getWords();
+			}
+		}
+		else if(_dbDriver)
+		{
+			std::list<int> ids;
+			ids.push_back(signatureId);
+			std::list<Signature *> signatures;
+			_dbDriver->loadKeypointSignatures(ids, signatures);
+			if(signatures.size())
+			{
+				const KeypointSignature * ks = dynamic_cast<const KeypointSignature*>(signatures.front());
+				if(ks)
+				{
+					words = ks->getWords();
+				}
+			}
+			for(std::list<Signature *>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
+			{
+				delete *iter;
+			}
+		}
+	}
+	return words;
+}
 
-			std::list<int> id;
-			id.push_back(sTo->id());
-			this->enableWordsRef(id);
-			// Set old image to new merged signature
-			sTo->setImage(sFrom->getImage());
-		}
-		else if(s == kUseOnlyDestMerging)
-		{
-			// do nothing... already "merged"
-		}
+std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore)
+{
+	if(!_tfIdfLikelihoodUsed)
+	{
+		return Memory::computeLikelihood(signature, ids, maximumScore);
 	}
 	else
 	{
-		ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
-	}
-	ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
-}
-
-std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signature, const std::set<int> & signatureIds) const
-{
-	//return Memory::computeLikelihood(signature, signatureIds);
-
-	// TODO cleanup , old way...
-	if(_tfIdfLikelihoodUsed)
-	{
+		// TODO cleanup , old way...
 		UTimer timer;
 		timer.start();
 		std::map<int, float> likelihood;
 		std::map<int, float> calculatedWordsRatio;
+		maximumScore = 0;
 
 		const KeypointSignature * newSurf = dynamic_cast<const KeypointSignature *>(signature);
 		if(!newSurf)
@@ -448,61 +416,34 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
 			ULOGGER_ERROR("The signature is not a KeypointSignature");
 			return likelihood; // Must be a KeypointSignature *
 		}
-
-		if(signatureIds.size() == 0)
+		else if(ids.empty())
 		{
-			const std::map<int, int> & wm = this->getWorkingMem();
-			for(std::map<int, int>::const_iterator iter = wm.begin(); iter!=wm.end(); ++iter)
-			{
-				likelihood.insert(likelihood.end(), std::pair<int, float>(iter->first, 0));
-				if(_tfIdfNormalized)
-				{
-					const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(iter->first));
-					float wordsCountRatio = -1; // default invalid
-					if(s)
-					{
-						if(s->getWords().size() > newSurf->getWords().size())
-						{
-							wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
-						}
-						else if(newSurf->getWords().size())
-						{
-							wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
-						}
-						calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
-					}
-					else
-					{
-						calculatedWordsRatio.insert(std::pair<int, float>(iter->first, wordsCountRatio));
-					}
-				}
-			}
+			UWARN("ids list is empty");
+			return likelihood;
 		}
-		else
+
+		for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			for(std::set<int>::const_iterator i=signatureIds.begin(); i != signatureIds.end(); ++i)
+			likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0));
+			if(_tfIdfNormalized)
 			{
-				likelihood.insert(likelihood.end(), std::pair<int, float>(*i, 0));
-				if(_tfIdfNormalized)
+				const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(*iter));
+				float wordsCountRatio = -1; // default invalid
+				if(s)
 				{
-					const KeypointSignature * s = dynamic_cast<const KeypointSignature *>(this->getSignature(*i));
-					float wordsCountRatio = -1; // default invalid
-					if(s)
+					if(s->getWords().size() > newSurf->getWords().size())
 					{
-						if(s->getWords().size() > newSurf->getWords().size())
-						{
-							wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
-						}
-						else if(newSurf->getWords().size())
-						{
-							wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
-						}
-						calculatedWordsRatio.insert(std::pair<int, float>(*i, wordsCountRatio));
+						wordsCountRatio = float(newSurf->getWords().size()) / float(s->getWords().size());
 					}
-					else
+					else if(newSurf->getWords().size())
 					{
-						calculatedWordsRatio.insert(std::pair<int, float>(*i, wordsCountRatio));
+						wordsCountRatio = float(s->getWords().size()) / float(newSurf->getWords().size());
 					}
+					calculatedWordsRatio.insert(std::pair<int, float>(*iter, wordsCountRatio));
+				}
+				else
+				{
+					calculatedWordsRatio.insert(std::pair<int, float>(*iter, wordsCountRatio));
 				}
 			}
 		}
@@ -518,7 +459,7 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
 		const VisualWord * vw;
 		float normalizationRatio;
 
-		N = likelihood.size();
+		N = this->getSignatures().size();
 
 		if(N)
 		{
@@ -572,15 +513,12 @@ std::map<int, float> KeypointMemory::computeLikelihood(const Signature * signatu
 					}
 				}
 			}
+			maximumScore = log(N);
 		}
 
-		ULOGGER_DEBUG("compute likelihood... %f s", timer.ticks());
+		ULOGGER_DEBUG("compute likelihood, maximumScore=%f... %f s", maximumScore, timer.ticks());
 		return likelihood;
 	}
-	else
-	{
-		return Memory::computeLikelihood(signature, signatureIds);
-	}
 
 }
 
@@ -599,11 +537,34 @@ int KeypointMemory::getNi(int signatureId) const
 	return ni;
 }
 
+void KeypointMemory::copyData(const Signature * from, Signature * to)
+{
+	// The signatures must be KeypointSignature
+	const KeypointSignature * sFrom = dynamic_cast<const KeypointSignature *>(from);
+	KeypointSignature * sTo = dynamic_cast<KeypointSignature *>(to);
+	UTimer timer;
+	timer.start();
+	if(sFrom && sTo)
+	{
+		this->disableWordsRef(sTo->id());
+		sTo->setWords(sFrom->getWords());
+
+		std::list<int> id;
+		id.push_back(sTo->id());
+		this->enableWordsRef(id);
+	}
+	else
+	{
+		ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
+	}
+	ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
+}
+
 class PreUpdateThread : public UThreadNode
 {
 public:
 	PreUpdateThread(VWDictionary * vwp) : _vwp(vwp) {}
-	~PreUpdateThread() {}
+	virtual ~PreUpdateThread() {}
 private:
 	void mainLoop() {
 		if(_vwp)
@@ -621,8 +582,8 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 
 	UTimer timer;
 	timer.start();
-	std::list<cv::KeyPoint> keypoints;
-	std::list<std::vector<float> > descriptors;
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat descriptors;
 	const IplImage * image = 0;
 
 	if(smState)
@@ -657,7 +618,7 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 		}
 		else
 		{
-			if(smState->getSensors().size() >= _badSignRatio * nbCommonWords)
+			if(smState->getSensors().rows >= _badSignRatio * nbCommonWords)
 			{
 				descriptors = smState->getSensors();
 				keypoints = smState->getKeypoints();
@@ -672,62 +633,29 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 	}
 
 	std::list<int> wordIds;
-	if(descriptors.size())
+	if(descriptors.rows)
 	{
-		unsigned int descriptorSize = descriptors.begin()->size();
 		if(_parallelized)
 		{
-			ULOGGER_DEBUG("time descriptor and memory update (%d of size=%d) = %fs", (int)descriptors.size(), (int)descriptorSize, timer.ticks());
+			ULOGGER_DEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
 		}
 		else
 		{
-			ULOGGER_DEBUG("time descriptor (%d of size=%d) = %fs", (int)descriptors.size(), (int)descriptorSize, timer.ticks());
+			ULOGGER_DEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
 		}
 
-		//append actuators
-		if(!_sensorStateOnly && smState->getActuators().size())
-		{
-			const std::list<std::vector<float> > & actuators = smState->getActuators();
-
-			unsigned int actuatorSize = actuators.begin()->size();
-			if(actuatorSize > descriptorSize)
-			{
-				UERROR("Actuator's size (%d) is larger than descriptor size (%d)", actuatorSize, descriptorSize);
-			}
-
-			for(std::list<std::vector<float> >::const_iterator iter = actuators.begin(); iter!=actuators.end(); ++iter)
-			{
-				std::vector<float> descriptor(descriptorSize);
-				// normalize actuator values
-				std::vector<float> actuatorNormalized = uNormalize(*iter);
-				for(unsigned int i=0; i<descriptorSize; ++i)
-				{
-					if(i<actuatorSize)
-					{
-						descriptor[i] = actuatorNormalized[i];
-					}
-					else
-					{
-						descriptor[i] = 0;
-					}
-				}
-				descriptors.push_back(descriptor);
-			}
-			ULOGGER_DEBUG("time setup actuators (%d of length %d) like descriptors %fs", (int)actuators.size(), (int)actuatorSize, timer.ticks());
-		}
-
-		wordIds = _vwd->addNewWords(descriptors, descriptorSize, id);
+		wordIds = _vwd->addNewWords(descriptors, id);
 		ULOGGER_DEBUG("time addNewWords %fs", timer.ticks());
 	}
-	else
+	else if(id>0)
 	{
-		ULOGGER_WARN("id %d is a bad signature", id);
+		UDEBUG("id %d is a bad signature", id);
 	}
 
 	std::multimap<int, cv::KeyPoint> words;
 	if(wordIds.size() > 0)
 	{
-		std::list<cv::KeyPoint>::iterator kpIter = keypoints.begin();
+		std::vector<cv::KeyPoint>::iterator kpIter = keypoints.begin();
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
 		{
 			if(kpIter != keypoints.end())
@@ -737,6 +665,7 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 			}
 			else
 			{
+				UWARN("Words (%d) and keypoints(%d) are not the same size ?!?", (int)wordIds.size(), (int)keypoints.size());
 				words.insert(std::pair<int, cv::KeyPoint >(*iter, cv::KeyPoint()));
 			}
 		}
@@ -787,7 +716,7 @@ void KeypointMemory::disableWordsRef(int signatureId)
 
 void KeypointMemory::cleanUnusedWords()
 {
-	ULOGGER_DEBUG("");
+	UINFO("");
 	if(_vwd->isIncremental())
 	{
 		std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
@@ -799,7 +728,7 @@ void KeypointMemory::cleanUnusedWords()
 
 			for(unsigned int i=0; i<removedWords.size(); ++i)
 			{
-				if(_dbDriver)
+				if(_dbDriver && !removedWords[i]->isSaved())
 				{
 					_dbDriver->asyncSave(removedWords[i]);
 				}
@@ -809,7 +738,7 @@ void KeypointMemory::cleanUnusedWords()
 				}
 			}
 		}
-		ULOGGER_DEBUG("%d words removed...", removedWords.size());
+		UINFO("%d words removed...", removedWords.size());
 	}
 }
 
@@ -834,23 +763,9 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
 			//Find words in the signature which they are not in the current dictionary
 			for(std::list<int>::const_iterator k=uniqueKeys.begin(); k!=uniqueKeys.end(); ++k)
 			{
-				if(_vwd->getWord(*k) == 0)
+				if(_vwd->getWord(*k) == 0 && _vwd->getUnusedWord(*k) == 0)
 				{
-					//std::map<int,int>::iterator iter = _wordRefsToChange.find(*k);
-					//if(iter != _wordRefsToChange.end())
-					//{
-					//	ss->changeWordsRef(iter->first, iter->second);
-					//	uniqueKeys.push_back(iter->second);
-					//}
-					//else
-					if(oldWordIds.find(*k) == oldWordIds.end())
-					{
-						oldWordIds.insert(oldWordIds.end(), *k);
-					}
-					else
-					{
-						//UDEBUG("*k=%d", *k);
-					}
+					oldWordIds.insert(oldWordIds.end(), *k);
 				}
 			}
 		}
@@ -862,7 +777,8 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
 	std::list<VisualWord *> vws;
 	if(oldWordIds.size() && _dbDriver)
 	{
-		_dbDriver->loadWords(std::list<int>(oldWordIds.begin(), oldWordIds.end()), vws); // get the descriptors
+		// get the descriptors
+		_dbDriver->loadWords(std::list<int>(oldWordIds.begin(), oldWordIds.end()), vws);
 	}
 	ULOGGER_DEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
 
@@ -879,11 +795,11 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
 			{
 				//ULOGGER_DEBUG("Match found %d with %d", (*iterVws)->id(), vwActiveIds[i]);
 				refsToChange.insert(refsToChange.end(), std::pair<int, int>((*iterVws)->id(), vwActiveIds[i]));
-				if((*iterVws)->isSaved() || !_dbDriver)
+				if((*iterVws)->isSaved())
 				{
 					delete (*iterVws);
 				}
-				else
+				else if(_dbDriver)
 				{
 					_dbDriver->asyncSave(*iterVws);
 				}
@@ -891,7 +807,7 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
 			else
 			{
 				//add to dictionary
-				_vwd->addWord(*iterVws);
+				_vwd->addWord(*iterVws); // take ownership
 			}
 			++i;
 		}
@@ -930,7 +846,7 @@ void KeypointMemory::enableWordsRef(const std::list<int> & signatureIds)
 	ULOGGER_DEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
 }
 
-int KeypointMemory::forget(const std::list<int> & ignoredIds)
+int KeypointMemory::forget(const std::set<int> & ignoredIds)
 {
 	ULOGGER_DEBUG("");
 	int signaturesRemoved = 0;
@@ -946,12 +862,20 @@ int KeypointMemory::forget(const std::list<int> & ignoredIds)
 		// dictionary to respect the limit.
 		while(wordsRemoved < newWords)
 		{
-			KeypointSignature *  s = dynamic_cast<KeypointSignature *>(this->getRemovableSignature(ignoredIds));
-			if(s)
+			std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
+			if(signatures.size())
 			{
-				++signaturesRemoved;
-				this->moveToTrash(s);
-				wordsRemoved = _vwd->getUnusedWordsSize();
+				KeypointSignature *  s = dynamic_cast<KeypointSignature *>(signatures.front());
+				if(s)
+				{
+					++signaturesRemoved;
+					this->moveToTrash(s);
+					wordsRemoved = _vwd->getUnusedWordsSize();
+				}
+				else
+				{
+					break;
+				}
 			}
 			else
 			{
@@ -968,7 +892,7 @@ int KeypointMemory::forget(const std::list<int> & ignoredIds)
 	return signaturesRemoved;
 }
 
-int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched)
+std::set<int> KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
 {
 	// get the signatures, if not in the working memory, they
 	// will be loaded from the database in an more efficient way
@@ -977,7 +901,6 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
 	ULOGGER_DEBUG("");
 	UTimer timer;
 	std::list<int> idsToLoad;
-	unsigned int touched = 0;
 	std::map<int, int>::iterator wmIter;
 	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
 	{
@@ -985,16 +908,10 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
 		{
 			if(!maxLoaded || idsToLoad.size() < maxLoaded)
 			{
-				//When loaded from the long-term memory, the signature
-				// is automatically added on top of the working memory
 				idsToLoad.push_back(*i);
+				UINFO("Loading location %d from database...", *i);
 			}
 		}
-		else if(touched < maxTouched)
-		{
-			this->touch(*i);
-		}
-		++touched;
 	}
 
 	ULOGGER_DEBUG("idsToLoad = %d", idsToLoad.size());
@@ -1002,8 +919,9 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
 	std::list<Signature *> reactivatedSigns;
 	if(_dbDriver)
 	{
-		_dbDriver->loadKeypointSignatures(idsToLoad, reactivatedSigns, true);
+		_dbDriver->loadKeypointSignatures(idsToLoad, reactivatedSigns);
 	}
+	timeDbAccess = timer.getElapsedTime();
 	std::list<int> idsLoaded;
 	for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
 	{
@@ -1013,7 +931,7 @@ int KeypointMemory::reactivateSignatures(const std::list<int> & ids, unsigned in
 	}
 	this->enableWordsRef(idsLoaded);
 	ULOGGER_DEBUG("time = %fs", timer.ticks());
-	return reactivatedSigns.size();
+	return std::set<int>(idsToLoad.begin(), idsToLoad.end());
 }
 
 void KeypointMemory::moveToTrash(Signature * s)

corelib/src/KeypointMemory.h

@@ -23,6 +23,7 @@
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include "Memory.h"
+#include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
 
@@ -34,8 +35,8 @@ class KeypointDescriptor;
 class RTABMAP_EXP KeypointMemory : public Memory
 {
 public:
-	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
-	enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
+	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
+	enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
 
 public:
 	KeypointMemory(const ParametersMap & parameters = ParametersMap());
@@ -43,9 +44,9 @@ public:
 
 	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 std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
+	virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign) const;
 
@@ -54,6 +55,7 @@ public:
 	const KeypointDetector * getKeypointDetector() const {return _keypointDetector;}
 	const KeypointDescriptor * getKeypointDescriptor() const {return _keypointDescriptor;}
 	const VWDictionary * getVWD() const {return _vwd;}
+	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
 	DetectorStrategy detectorStrategy() const;
 
 protected:
@@ -62,10 +64,11 @@ protected:
 	virtual void clear();
 	virtual void moveToTrash(Signature * s);
 	virtual void preUpdate();
-	virtual void merge(const Signature * from, Signature * to, MergingStrategy s);
 
 private:
+	virtual void copyData(const Signature * from, Signature * to);
 	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
+
 	void disableWordsRef(int signatureId);
 	void enableWordsRef(const std::list<int> & signatureIds);
 	void cleanUnusedWords();
@@ -81,7 +84,6 @@ private:
 	float _badSignRatio;;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
-	bool _sensorStateOnly;
 	bool _tfIdfNormalized;
 };
 

corelib/src/Memory.h

@@ -35,6 +35,7 @@
 namespace rtabmap {
 
 class Signature;
+class NeighborLink;
 class DBDriver;
 class Node;
 class SMState;
@@ -46,8 +47,6 @@ public:
 	static const int kIdVirtual;
 	static const int kIdInvalid;
 
-	enum MergingStrategy{kUseOnlyFromMerging, kUseOnlyDestMerging};
-
 public:
 	Memory(const ParametersMap & parameters = ParametersMap());
 	virtual ~Memory();
@@ -55,31 +54,34 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 	bool update(const SMState * rawData, std::map<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);
+	virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
+	virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
 	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;
+	bool addLoopClosureLink(int oldId, int newId);
+	std::map<int, int> getNeighborsId(double & dbAccessTime, int signatureId, unsigned int margin, int maxCheckedInDatabase = -1, bool onlyWithActions = false, bool incrementMarginOnLoop = false, bool ignoreSTM = true, bool ignoreLoopIds = false) const;
+	float compareOneToOne(const std::vector<int> & idsA, const std::vector<int> & idsB);
 
 	//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> & getWorkingMem() const {return _workingMem;}
 	const std::set<int> & getStMem() const {return _stMem;}
-	std::list<int> getChildrenIds(int signatureId) const;
+	std::list<NeighborLink> getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure = false, bool lookInDatabase = false) const;
+	void getLoopClosureIds(int signatureId, std::set<int> & loopClosureIds, std::set<int> & childLoopClosureIds, bool lookInDatabase = false) const;
 	bool isRawDataKept() const {return _rawDataKept;}
+	float getSimilarityThr() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getWeight(int id) const;
+	const std::vector<int> & getLastBaseIds() const {return _lastBaseIds;}
 	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;}
@@ -93,7 +95,6 @@ public:
 	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;}
@@ -109,7 +110,6 @@ public:
 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();
@@ -118,41 +118,45 @@ protected:
 
 	void addSignatureToWm(Signature * signature);
 	Signature * _getSignature(int id) const;
-	Signature * _getLastSignature();
-	Signature * getRemovableSignature(const std::list<int> & ignoredIds = std::list<int>(), bool onlyLoopedSignatures = false);
+	std::list<Signature *> getRemovableSignatures(int count, const std::set<int> & ignoredIds = std::set<int>());
 	int getNextId();
 	void initCountId();
-	int rehearsal(const Signature * signature, bool onlyLast, float & similarity);
-	void touch(int signatureId);
+	void rehearsal(Signature * signature, std::map<std::string, float> & stats);
+
 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
 
 private:
-	void createVirtualSignature(Signature ** signature);
+	virtual void copyData(const Signature * from, Signature * to) = 0;
 	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false) = 0;
+
+	void createVirtualSignature(Signature ** signature);
 	void cleanGraph(const Node * root);
 protected:
 	DBDriver * _dbDriver;
 
 private:
+	// parameters
 	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 _dataMergedOnRehearsal;
+
+	int _idCount;
+	Signature * _lastSignature;
+	int _lastLoopClosureId;
 	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
+	std::set<int> _stMem; // id
+	std::set<int> _workingMem; // id,age
+	std::vector<int> _lastBaseIds;
+	std::map<int, std::map<int, float> > _similaritiesMap;
 };
 
 } // namespace rtabmap

corelib/src/Parameters.cpp

@@ -24,9 +24,8 @@
 namespace rtabmap
 {
 
-Parameters * Parameters::instance_ = 0;
-UDestroyer<Parameters> Parameters::destroyer_;
 ParametersMap Parameters::parameters_;
+Parameters Parameters::instance_;
 
 Parameters::Parameters()
 {
@@ -37,30 +36,10 @@ 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();

corelib/src/RtabmapEvent.cpp

@@ -115,7 +115,6 @@ void Statistics::setLoopClosureImage(const IplImage * loopClosureImage)
 
 Statistics & Statistics::operator=(const Statistics & s)
 {
-	ULOGGER_DEBUG("");
 	_data = s.data();
 	if(_refImage)
 	{
@@ -143,7 +142,9 @@ Statistics & Statistics::operator=(const Statistics & s)
 	_weights = s.weights();
 	_refWords = s.refWords();
 	_loopWords = s.loopWords();
-
+	_refMotionMask = s.refMotionMask();
+	_loopMotionMask = s.loopMotionMask();
+	_actions = s.getActions();
 	return *this;
 }
 

corelib/src/SMMemory.cpp

@@ -0,0 +1,607 @@
+/*
+ * 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 "SMMemory.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/core/DBDriver.h"
+#include "utilite/UtiLite.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SMState.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#include "utilite/UStl.h"
+#include "utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/core/core.hpp>
+#include <set>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include "ColorTable.h"
+
+namespace rtabmap {
+
+
+SMMemory::SMMemory(const ParametersMap & parameters) :
+	Memory(parameters),
+	_useLogPolar(Parameters::defaultSMLogPolarUsed()),
+	_useVotingScheme(Parameters::defaultSMVotingSchemeUsed()),
+	_colorTable(0),
+	_useMotionMask(Parameters::defaultSMMotionMaskUsed())
+{
+	this->parseParameters(parameters);
+	if(!_colorTable)
+	{
+		int i=1;
+		this->setColorTable(i<<(Parameters::defaultSMColorTable() + 3));
+	}
+}
+
+SMMemory::~SMMemory()
+{
+	ULOGGER_DEBUG("");
+	if(this->memoryChanged())
+	{
+		this->clear();
+	}
+	delete _colorTable;
+}
+
+void SMMemory::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSMLogPolarUsed())) != parameters.end())
+	{
+		_useLogPolar = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSMVotingSchemeUsed())) != parameters.end())
+	{
+		this->setVotingScheme(uStr2Bool((*iter).second.c_str()));
+	}
+	if((iter=parameters.find(Parameters::kSMMotionMaskUsed())) != parameters.end())
+	{
+		_useMotionMask = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSMColorTable())) != parameters.end())
+	{
+		// index 0 = 8, index 1 = 16...
+		if(atoi((*iter).second.c_str()) == 8)
+		{
+			setColorTable(65536);
+		}
+		else
+		{
+			int i=1;
+			setColorTable(i<<(atoi((*iter).second.c_str()) + 3));
+		}
+	}
+
+	Memory::parseParameters(parameters);
+}
+
+void SMMemory::setVotingScheme(bool useVotingScheme)
+{
+	_useVotingScheme = useVotingScheme;
+	_dictionary.clear();
+	if(_useVotingScheme)
+	{
+		const std::map<int, Signature *> & signatures = this->getSignatures();
+		for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+		{
+			this->updateDictionary(i->second);
+		}
+	}
+}
+
+void SMMemory::setColorTable(int size)
+{
+	if(_colorTable)
+	{
+		if(_colorTable->size() != size)
+		{
+			delete _colorTable;
+			_colorTable = new ColorTable(size);
+		}
+	}
+	else
+	{
+		_colorTable = new ColorTable(size);
+	}
+}
+
+void SMMemory::copyData(const Signature * from, Signature * to)
+{
+	// The signatures must be SMSignature
+	const SMSignature * sFrom = dynamic_cast<const SMSignature *>(from);
+	SMSignature * sTo = dynamic_cast<SMSignature *>(to);
+	UTimer timer;
+	timer.start();
+	if(sFrom && sTo)
+	{
+		sTo->setSensors(sFrom->getSensors());
+		sTo->setMotionMask(sFrom->getMotionMask());
+	}
+	else
+	{
+		ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
+	}
+	ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
+}
+
+Signature * SMMemory::createSignature(int id, const SMState * smState, bool keepRawData)
+{
+	UDEBUG("");
+	UTimer timer;
+	timer.start();
+	UTimer timerDetails;
+	timerDetails.start();
+	std::vector<int> sensors;
+	const std::vector<int> * sensorsPrevious = 0;
+	std::vector<unsigned char> motionMask;
+	const IplImage * image = 0;
+	IplImage * polar = 0;
+	IplImage * indexed = 0;
+	const SMSignature * previousSignature = dynamic_cast<const SMSignature *>(this->getLastSignature());
+	if(previousSignature)
+	{
+		UDEBUG("");
+		sensorsPrevious = &previousSignature->getSensors();
+	}
+	if(smState)
+	{
+		image = smState->getImage();
+
+		// sensors
+		if(!smState->getSensors().empty() == 0 && image && image->imageSize)
+		{
+			if(image->depth != IPL_DEPTH_8U && image->nChannels != 3)
+			{
+				UFATAL("Only IplImage depth of IPL_DEPTH_8U and 3 channels (BGR) is supported.");
+			}
+
+			UDEBUG("depth=%d, alpha=%d, widthStep=%d, width=%d, height=%d, nChannels=%d, imageSize=%d,", image->depth, image->alphaChannel, image->widthStep, image->width, image->height, image->nChannels, image->imageSize);
+
+			if(_useLogPolar)
+			{
+				// Log-polar transform
+				int radius = image->height < image->width ? image->height/2: image->width/2;
+				CvSize polarSize = cvSize(64, 128);
+				float M = polarSize.width/std::log(radius);
+				polar = cvCreateImage( polarSize, 8, 3 );
+				cvLogPolar( image, polar, cvPoint2D32f(image->width/2,image->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
+
+				UDEBUG("polar size= %d, %d, time=%fs", polar->width, polar->height, timerDetails.ticks());
+
+				// IND transform
+				unsigned char * data = (unsigned char *)polar->imageData;
+				sensors = std::vector<int>(polar->width*polar->height);
+				if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensors.size()))
+				{
+					_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
+				}
+				if(_useMotionMask)
+				{
+					motionMask = std::vector<unsigned char>(sensors.size(), 0);
+				}
+				bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
+				int k=0;
+				for(int i=0; i<polar->height; ++i)
+				{
+					for(int j=0; j<polar->width; ++j)
+					{
+						unsigned char & b = data[i*polar->widthStep+j*3+0];
+						unsigned char & g = data[i*polar->widthStep+j*3+1];
+						unsigned char & r = data[i*polar->widthStep+j*3+2];
+						int index = (int)_colorTable->getIndex(r, g, b);
+						sensors[k] = index;
+						_colorTable->getRgb(index, r, g , b);
+
+						if(_useMotionMask && updateMask && sensorsPrevious->at(k) != sensors[k])
+						{
+							motionMask[k] = 1;
+						}
+
+						if(!_dictionary.empty())
+						{
+							std::set<int> sensorId;
+							sensorId.insert(id);
+							std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
+							ret = _dictionary[k].insert(std::make_pair(sensors[k], sensorId));
+							if(ret.second == false)
+							{
+								ret.first->second.insert(id);
+							}
+						}
+
+						++k;
+					}
+				}
+
+				UDEBUG("indexing time = %fs", timerDetails.ticks());
+
+				//cv::Mat indPolar;
+				//fromIndPolar = cvCreateImage(cvGetSize(image), 8, 3);
+				//cvLogPolar(polar, fromIndPolar, cvPoint2D32f(image->width/2,image->height/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS+CV_WARP_INVERSE_MAP );
+				//UDEBUG("back from polar time = %fs", timerDetails());
+
+				//image = polar;
+			}
+			else
+			{
+				// IND transform
+				indexed = cvCloneImage(image);
+				unsigned char * data = (unsigned char *)indexed->imageData;
+				sensors = std::vector<int>(indexed->width*indexed->height);
+				if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensors.size()))
+				{
+					_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
+				}
+				if(_useMotionMask)
+				{
+					motionMask = std::vector<unsigned char>(sensors.size(), 0);
+				}
+				bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
+				int k=0;
+				int sum=0;
+				for(int i=0; i<indexed->height; ++i)
+				{
+					for(int j=0; j<indexed->width; ++j)
+					{
+						unsigned char & b = data[i*indexed->widthStep+j*3+0];
+						unsigned char & g = data[i*indexed->widthStep+j*3+1];
+						unsigned char & r = data[i*indexed->widthStep+j*3+2];
+						int index = (int)_colorTable->getIndex(r, g, b);
+						sensors[k] = index;
+						_colorTable->getRgb(index, r, g , b);
+
+						if(_useMotionMask && updateMask && sensorsPrevious->at(k) != sensors[k])
+						{
+							motionMask[k] = 1;
+							++sum;
+						}
+
+						if(!_dictionary.empty())
+						{
+							std::set<int> sensorId;
+							sensorId.insert(id);
+							std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
+							ret = _dictionary[k].insert(std::make_pair(sensors[k], sensorId));
+							if(ret.second == false)
+							{
+								ret.first->second.insert(id);
+							}
+						}
+
+						++k;
+					}
+				}
+				image = indexed;
+
+				UDEBUG("sum=%d, indexing time = %fs", sum, timerDetails.ticks());
+			}
+		}
+		else
+		{
+			std::vector<float> sensorsMerged;
+			int buf;
+			smState->getSensorsMerged(sensorsMerged, buf);
+			sensors = std::vector<int>(sensorsMerged.size());
+			if(_useVotingScheme && (_dictionary.empty() || _dictionary.size() != sensorsMerged.size()))
+			{
+				_dictionary = std::vector<std::map<int, std::set<int> > >(sensorsMerged.size());
+			}
+			if(_useMotionMask)
+			{
+				motionMask = std::vector<unsigned char>(sensors.size(), 0);
+			}
+			bool updateMask = sensorsPrevious && sensorsPrevious->size() == motionMask.size();
+			for(unsigned int i=0; i<sensorsMerged.size(); ++i)
+			{
+				if(sensorsMerged[i]>0 && sensorsMerged[i]<1)
+				{
+					UWARN("Conversion from float to int may lost precision...");
+				}
+				sensors[i] = (int)sensorsMerged[i];
+				if(_useMotionMask && updateMask && sensorsPrevious->at(i) != sensors[i])
+				{
+					motionMask[i] = 1;
+				}
+
+				if(!_dictionary.empty())
+				{
+					std::set<int> sensorId;
+					sensorId.insert(id);
+					std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
+					ret = _dictionary[i].insert(std::make_pair(sensors[i], sensorId));
+					if(ret.second == false)
+					{
+						ret.first->second.insert(id);
+					}
+				}
+			}
+		}
+	}
+	SMSignature * s = new SMSignature(sensors, motionMask, id, image, keepRawData);
+
+	if(polar)
+	{
+		cvReleaseImage(&polar);
+	}
+	if(indexed)
+	{
+		cvReleaseImage(&indexed);
+	}
+
+	ULOGGER_DEBUG("time new signature (id=%d) %fs", id, timer.ticks());
+	return s;
+}
+
+std::map<int, float> SMMemory::computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore)
+{
+	if(!_useVotingScheme)
+	{
+		return Memory::computeLikelihood(signature, ids, maximumScore);
+	}
+	else
+	{
+		UTimer timer;
+		timer.start();
+		std::map<int, float> likelihood;
+		maximumScore = 0;
+
+		const SMSignature * query = dynamic_cast<const SMSignature *>(signature);
+		if(!query)
+		{
+			ULOGGER_ERROR("The signature is not a SMSignature");
+			return likelihood; // Must be a SMSignature *
+		}
+		else if(ids.empty())
+		{
+			UWARN("ids list is empty");
+			return likelihood;
+		}
+
+		UDEBUG("Likelihood for %d", query->id());
+
+		const std::vector<int> & sensors = query->getSensors();
+		if(_dictionary.size() != sensors.size())
+		{
+			UERROR("Dictionary (%d) and sensor (%d) are not the same size!", (int)_dictionary.size(), (int)sensors.size());
+			return likelihood;
+		}
+		const std::vector<unsigned char> & mask = query->getMotionMask();
+		bool maskUsed = false;
+		if(mask.size() != 0 && mask.size() != sensors.size())
+		{
+			UWARN("mask's size (%d) and sensor's size (%d) are not equal", (int)mask.size(), (int)sensors.size());
+		}
+		else if(mask.size())
+		{
+			maskUsed = true;
+		}
+
+		// prepare likelihood
+		for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+		{
+			likelihood.insert(likelihood.end(), std::make_pair(*iter, 0.0f));
+		}
+
+		//float nwi; // nwi is the number of a specific word referenced by a place
+		//float ni; // ni is the total of words referenced by a place
+		float nw; // nw is the number of places referenced by a specific word
+		float N; // N is the total number of places
+		float logNnw;
+
+		N = this->getSignatures().size();
+
+		if(N)
+		{
+			for(unsigned int i=0; i<sensors.size(); ++i)
+			{
+				if(!maskUsed || mask[i])
+				{
+					// "Inverted index"
+					std::map<int, std::set<int> >::iterator iter = _dictionary[i].find(sensors[i]);
+					if(iter == _dictionary[i].end())
+					{
+						UERROR("Sensor %d not found in dictionary ?!?", sensors[i]);
+					}
+					else
+					{
+						nw = iter->second.size();
+						if(nw)
+						{
+							if(nw > N)
+							{
+								for(std::set<int>::iterator jter = iter->second.begin(); jter!=iter->second.end(); ++jter)
+								{
+									UERROR("sensor pos %d, refid = %d", (int)i, *jter);
+								}
+
+								UFATAL("id=%d, N = %f, nw=%f", signature->id(), N, nw);
+							}
+							logNnw = log10(N/nw);
+							if(logNnw)
+							{
+								for(std::set<int>::iterator jter = iter->second.begin(); jter!=iter->second.end(); ++jter)
+								{
+									std::map<int, float>::iterator kter = likelihood.find(*jter);
+									if(kter != likelihood.end())
+									{
+										kter->second += logNnw;
+									}
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+
+		if(sensors.size())
+		{
+			maximumScore = log(N) * float(sensors.size());
+		}
+
+		ULOGGER_DEBUG("compute likelihood, maximumScore=%f... %f s", maximumScore, timer.ticks());
+		return likelihood;
+	}
+}
+
+void SMMemory::moveToTrash(Signature * s)
+{
+	if(_useVotingScheme)
+	{
+		UTimer timer;
+		SMSignature * sm = dynamic_cast<SMSignature *>(s);
+		if(sm && sm->id() > 0)
+		{
+			const std::vector<int> & sensors = sm->getSensors();
+			if(sensors.size() == _dictionary.size())
+			{
+				for(unsigned int i=0; i<sensors.size(); ++i)
+				{
+					std::map<int, std::set<int> >::iterator iter = _dictionary[i].find(sensors[i]);
+					if(iter != _dictionary[i].end())
+					{
+						if(!iter->second.erase(sm->id()))
+						{
+							UWARN("Sensor id %d not found in dictionary at pos %d", sm->id(), (int)i);
+						}
+					}
+					else
+					{
+						UWARN("Sensor value %d at sensor pos %d is not found in dictionary", sensors[i], (int)i);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Dictionary size (%d) is not the same as the sensor (%d), signId=%d", (int)_dictionary.size(), (int)sensors.size(), sm->id());
+			}
+		}
+		UDEBUG("time=%fs", timer.ticks());
+	}
+	Memory::moveToTrash(s);
+}
+
+Signature * SMMemory::getSignatureLtMem(int id)
+{
+	Signature * s = Memory::getSignatureLtMem(id);
+	if(_useVotingScheme && s)
+	{
+		this->updateDictionary(s);
+	}
+	return s;
+}
+
+bool SMMemory::init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
+{
+	UDEBUG("");
+	bool success = Memory::init(dbDriverName, dbUrl, dbOverwritten, parameters);
+
+	if(_useVotingScheme)
+	{
+		// Update sensory dictionary
+		const std::map<int, Signature *> & signatures = this->getSignatures();
+		for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
+		{
+			this->updateDictionary(i->second);
+		}
+	}
+
+	return success;
+}
+
+void SMMemory::updateDictionary(const Signature * s)
+{
+	if(s)
+	{
+		const SMSignature * sm = dynamic_cast<const SMSignature *>(s);
+		if(sm)
+		{
+			const std::vector<int> & sensors = sm->getSensors();
+			if(_dictionary.empty())
+			{
+				_dictionary = std::vector<std::map<int, std::set<int> > >(sensors.size());
+			}
+			if(sensors.size() == _dictionary.size())
+			{
+				for(unsigned int i=0; i<sensors.size(); ++i)
+				{
+					std::set<int> sensorId;
+					sensorId.insert(sm->id());
+					std::pair<std::map<int, std::set<int> >::iterator, bool> ret;
+					ret = _dictionary[i].insert(std::make_pair(sensors[i], sensorId));
+					if(ret.second == false)
+					{
+						ret.first->second.insert(sm->id());
+					}
+				}
+			}
+			else if(_dictionary.size())
+			{
+				UWARN("Loaded signature %d with size (%d) doesn't have the same size as the dicitonary (%d)", sm->id(), (int)sensors.size(), (int)_dictionary.size());
+			}
+		}
+	}
+	else
+	{
+		UFATAL("Signature must not be null!");
+	}
+}
+
+std::set<int> SMMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
+{
+	// get the signatures, if not in the working memory, they
+	// will be loaded from the database in an more efficient way
+	// than how it is done in the Memory
+
+	ULOGGER_DEBUG("");
+	UTimer timer;
+	std::list<int> idsToLoad;
+	std::map<int, int>::iterator wmIter;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
+		{
+			if(!maxLoaded || idsToLoad.size() < maxLoaded)
+			{
+				idsToLoad.push_back(*i);
+			}
+		}
+	}
+
+	ULOGGER_DEBUG("idsToLoad = %d", idsToLoad.size());
+
+	std::list<Signature *> reactivatedSigns;
+	if(_dbDriver)
+	{
+		_dbDriver->loadSMSignatures(idsToLoad, reactivatedSigns);
+	}
+	timeDbAccess = timer.getElapsedTime();
+	for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
+	{
+		//append to working memory
+		this->addSignatureToWm(*i);
+	}
+	ULOGGER_DEBUG("time = %fs", timer.ticks());
+	return std::set<int>(idsToLoad.begin(), idsToLoad.end());
+}
+
+} // namespace rtabmap

corelib/src/SMMemory.h

@@ -0,0 +1,64 @@
+/*
+ * 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 SIMPLEMEMORY_H_
+#define SIMPLEMEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "Memory.h"
+
+namespace rtabmap {
+
+class ColorTable;
+class SMSignature;
+
+class RTABMAP_EXP SMMemory : public Memory
+{
+public:
+	SMMemory(const ParametersMap & parameters = ParametersMap());
+	virtual ~SMMemory();
+	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::list<int> & ids, float & maximumScore);
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
+	void setRoi(const std::string & roi);
+	void setVotingScheme(bool useVotingScheme);
+	void setColorTable(int size);
+
+protected:
+	virtual void moveToTrash(Signature * s);
+	virtual Signature * getSignatureLtMem(int id);
+
+private:
+	virtual void copyData(const Signature * from, Signature * to);
+	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
+	void updateDictionary(const Signature * s);
+
+private:
+	bool _useLogPolar;
+	bool _useVotingScheme;
+	ColorTable * _colorTable;
+	bool _useMotionMask;
+	std::vector<std::map<int, std::set<int> > > _dictionary;
+};
+
+}
+
+#endif /* KEYPOINTMEMORY_H_ */

corelib/src/Signature.cpp

@@ -17,16 +17,36 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "Memory.h"
 #include <opencv2/highgui/highgui.hpp>
 #include "VerifyHypotheses.h"
+#include "rtabmap/core/SMState.h"
 
 #include "utilite/UtiLite.h"
 
 namespace rtabmap
 {
 
+bool NeighborLink::updateIds(int idFrom, int idTo)
+{
+	bool modified = false;
+	if(_id == idFrom)
+	{
+		_id = idTo;
+		modified = true;
+	}
+	for(unsigned int i=0; i<_baseIds.size(); ++i)
+	{
+		if(_baseIds[i] == idFrom)
+		{
+			_baseIds[i] = idTo;
+			modified = true;
+		}
+	}
+	return modified;
+}
+
 Signature::~Signature()
 {
 	ULOGGER_DEBUG("id=%d", _id);
@@ -39,16 +59,12 @@ Signature::~Signature()
 Signature::Signature(int id, const IplImage * image, bool keepImage) :
 		_id(id),
 		_weight(0),
-		_loopClosureId(0),
 		_image(0),
 		_saved(false),
-		_width(0),
-		_height(0)
+		_modified(true)
 {
 	if(image)
 	{
-		_width = image->width;
-		_height = image->height;
 		if(keepImage)
 		{
 			_image = cvCloneImage(image);
@@ -68,10 +84,11 @@ void Signature::setImage(const IplImage * image)
 	{
 		cvReleaseImage(&_image);
 		_image = cvCloneImage(image);
+		_modified = true;
 	}
 	else
 	{
-		UWARN("Parameter is null or no image is saved.");
+		UDEBUG("Parameter is null or no image is saved.");
 	}
 }
 
@@ -111,47 +128,60 @@ IplImage * Signature::decompressImage(const CvMat * imageCompressed)
 	return cvDecodeImage(imageCompressed, CV_LOAD_IMAGE_ANYCOLOR);
 }
 
-void Signature::addNeighbors(const NeighborsMap & neighbors)
+void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
 {
-	for(NeighborsMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	for(NeighborsMultiMap::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());
+		this->addNeighbor(i->second);
 	}
 }
 
-void Signature::addNeighbor(int neighbor, const std::list<std::vector<float> > & actions)
+void Signature::addNeighbor(const NeighborLink & neighbor)
 {
-	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)
+	UDEBUG("Add neighbor %d to %d", neighbor.id(), this->id());
+	/*std::string baseIdsDebug;
+	const std::vector<int> & baseIds = neighbor.baseIds();
+	for(unsigned int i=0; i<baseIds.size(); ++i)
 	{
-		ULOGGER_ERROR("neighbor %d already added to %d", neighbor, this->id());
-		return;
-	}
-	if(neighbor == _id)
-	{
-		ULOGGER_ERROR("same Id ? (%d)", neighbor, this->id());
-		return;
+		baseIdsDebug.append(uNumber2str(baseIds[i]));
+		if(i+1 < baseIds.size())
+		{
+			baseIdsDebug.append(", ");
+		}
 	}
+	UDEBUG("Adding neighbor %d to %d with %d actions, %d baseIds = [%s]", neighbor.id(), this->id(), neighbor.actions().size(), neighbor.baseIds().size(), baseIdsDebug.c_str());
+    */
+
+	_neighbors.insert(std::pair<int, NeighborLink>(neighbor.id(), neighbor));
+	_neighborsModified = true;
 }
 
-void Signature::removeNeighbor(int neighbor)
+void Signature::changeNeighborIds(int idFrom, int idTo)
 {
-	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)
+	std::pair<NeighborsMultiMap::iterator, NeighborsMultiMap::iterator> pair = _neighbors.equal_range(idFrom);
+
+	if(pair.first != _neighbors.end() && pair.first != pair.second)
 	{
-		ULOGGER_WARN("neighbor %d not found in %d", neighbor, this->id());
+		std::list<NeighborLink> linksToAdd;
+		for(NeighborsMultiMap::iterator iter = pair.first; iter!=pair.second; ++iter)
+		{
+			NeighborLink link = iter->second;
+			link.updateIds(idFrom, idTo);
+			linksToAdd.push_back(link);
+		}
+		_neighbors.erase(idFrom);
+		for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
+		{
+			_neighbors.insert(std::pair<int, NeighborLink>(iter->id(), *iter));
+		}
+		_modified = true;
+		_neighborsModified = true;
+		UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
 	}
 }
 
 
 
-
 //KeypointSignature
 KeypointSignature::KeypointSignature(
 		const std::multimap<int, cv::KeyPoint> & words,
@@ -191,19 +221,6 @@ float KeypointSignature::compareTo(const Signature * s) const
 			HypVerificatorEpipolarGeo::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;
@@ -215,6 +232,7 @@ void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
 	if(kps.size())
 	{
 		_words.erase(oldWordId);
+		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
 		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
 		{
 			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
@@ -240,4 +258,126 @@ void KeypointSignature::removeWord(int wordId)
 	_words.erase(wordId);
 }
 
+
+
+
+
+//SMSignature
+SMSignature::SMSignature(
+		const std::vector<int> & sensors,
+		const std::vector<unsigned char> & motionMask,
+		int id,
+		const IplImage * image,
+		bool keepRawData) :
+	Signature(id, image, keepRawData),
+	_sensors(sensors),
+	_motionMask(motionMask)
+{
+	if(_sensors.size() != _motionMask.size() && _motionMask.size() > 0)
+	{
+		UFATAL("Sensors and mask must have the same size (%d vs %d)", (int)_sensors.size(), (int)_motionMask.size());
+	}
+	UDEBUG("sensors=%d", (int)_sensors.size());
 }
+
+SMSignature::SMSignature(int id) :
+	Signature(id)
+{
+}
+
+SMSignature::~SMSignature()
+{
+}
+
+float SMSignature::compareTo(const Signature * s) const
+{
+	const SMSignature * sm = dynamic_cast<const SMSignature *>(s);
+	float similarity = 0;
+
+	if(sm)
+	{
+		const std::vector<int> & sensorsB = sm->getSensors();
+		const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
+
+		if(_sensors.size() == sensorsB.size() && _sensors.size()) //Compatible
+		{
+			bool appearanceOnly = false;
+			if(appearanceOnly)
+			{
+				std::multiset<int> sensorsSetA(_sensors.begin(), _sensors.end());
+				std::multiset<int> sensorsSetB(sensorsB.begin(), sensorsB.end());
+				std::set<int> ids(_sensors.begin(), _sensors.end());
+				std::multiset<int>::iterator iterA;
+				std::multiset<int>::iterator iterB;
+				float realPairsCount = 0;
+				for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
+				{
+					iterA = sensorsSetA.find(*i);
+					iterB = sensorsSetB.find(*i);
+					while(iterA != sensorsSetA.end() && iterB != sensorsSetB.end() && *iterA == *iterB && *iterA == *i)
+					{
+						++iterA;
+						++iterB;
+						++realPairsCount;
+					}
+				}
+				similarity = realPairsCount / float(_sensors.size());
+			}
+			else if(_motionMask.size() == _sensors.size() &&
+					_motionMask.size() == motionMaskB.size())
+			{
+				int sum = 0;
+				int maskSumA = 0;
+				int maskSumB = 0;
+
+				// compare sensors
+				for(unsigned int i=0; i<_sensors.size(); ++i)
+				{
+					maskSumA += _motionMask[i];
+					maskSumB += motionMaskB[i];
+					sum += _sensors.at(i) == sensorsB.at(i) && _motionMask[i] && motionMaskB[i] ? 1 : 0;
+				}
+
+				int totalSize = maskSumA>maskSumB?maskSumA:maskSumB;
+				if(totalSize)
+				{
+					similarity = float(sum)/float(totalSize);
+				}
+			}
+			else
+			{
+				int sum = 0;
+				// compare sensors
+				for(unsigned int i=0; i<_sensors.size(); ++i)
+				{
+					sum += _sensors.at(i) == sensorsB.at(i) ? 1 : 0;
+				}
+				similarity = float(sum)/float(_sensors.size());
+			}
+
+			if(similarity<0 || similarity>1)
+			{
+				UERROR("Something wrong! similarity is not between 0 and 1 (%f)", similarity);
+			}
+		}
+		else if(!s->isBadSignature() && !this->isBadSignature())
+		{
+			UWARN("Not compatible signatures : nb sensors A=%d B=%d", (int)_sensors.size(), (int)sensorsB.size());
+		}
+	}
+	else if(s)
+	{
+		UWARN("Only SM signatures are compared. (type tested=%s)", s->signatureType().c_str());
+	}
+	return similarity;
+}
+
+
+bool SMSignature::isBadSignature() const
+{
+	if(_sensors.size() == 0)
+		return true;
+	return false;
+}
+
+} //namespace rtabmap

corelib/src/Signature.h

@@ -1,131 +0,0 @@
-/*
- * 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.cpp

@@ -20,7 +20,7 @@
 #include "VWDictionary.h"
 
 #include "VisualWord.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/DBDriver.h"
 #include "NearestNeighbor.h"
 #include "rtabmap/core/Parameters.h"
@@ -406,24 +406,23 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 	}
 }
 
-std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > & descriptors,
-							   unsigned int dim,
+std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 							   int signatureId)
 {
 	UTimer timer;
 	std::list<int> wordIds;
 	ULOGGER_DEBUG("");
-	if(_dim && _dim != dim && dim)
+	if(_dim && _dim != descriptors.cols && descriptors.cols)
 	{
-		ULOGGER_WARN("Descriptor size has changed! (%d to %d), Nearest neighbor approaches may not work with different descriptor sizes.", _dim, dim);
+		ULOGGER_WARN("Descriptor size has changed! (%d to %d), Nearest neighbor approaches may not work with different descriptor sizes.", _dim, descriptors.cols);
 	}
-	else if(!dim)
+	else if(!descriptors.cols)
 	{
 		ULOGGER_ERROR("Descriptor size is null?!?");
 		return wordIds;
 	}
-	_dim = dim;
-	if (descriptors.size() == 0 || !_dim)
+	_dim = descriptors.cols;
+	if (descriptors.empty() || !_dim)
 	{
 		ULOGGER_ERROR("Parameters don't fit the requirements of this method");
 		return wordIds;
@@ -442,31 +441,24 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
 	{
 		std::list<VisualWord *> newWords;
 
-		cv::Mat results(descriptors.size(), k, CV_32SC1); // results index
+		cv::Mat results(descriptors.rows, k, CV_32SC1); // results index
 		cv::Mat dists;
 		if(_nn->isDist64F())
 		{
-			dists = cv::Mat(descriptors.size(), k, CV_64FC1); // Distance results are CV_64FC1;
+			dists = cv::Mat(descriptors.rows, k, CV_64FC1); // Distance results are CV_64FC1;
 		}
 		else
 		{
-			dists = cv::Mat(descriptors.size(), k, CV_32FC1); // Distance results are CV_32FC1
+			dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
 		}
-		cv::Mat newPts(descriptors.size(), _dim, CV_32F); // SURF descriptors are CV_32F
-
-		// fill the request matrix
-		std::list<std::vector<float> >::const_iterator itDesc = descriptors.begin();
-		for(unsigned int i=0; i<descriptors.size(); ++itDesc, ++i)
+		cv::Mat newPts; // SURF descriptors are CV_32F
+		if(descriptors.type()!=CV_32F)
 		{
-			float * rowFl = newPts.ptr<float>(i);
-			if(itDesc->size() == _dim)
-			{
-				memcpy(rowFl, (const float *)itDesc->data(), _dim*sizeof(float));
-			}
-			else
-			{
-				ULOGGER_WARN("Descriptors are not the same size! The result may be wrong...");
-			}
+			descriptors.convertTo(newPts, CV_32F); // make sure it's CV_32F
+		}
+		else
+		{
+			newPts = descriptors;
 		}
 
 		UTimer timerLocal;
@@ -480,7 +472,7 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
 		}
 
 		//
-		for(unsigned int i = 0; i < descriptors.size(); ++i)
+		for(int i = 0; i < descriptors.rows; ++i)
 		{
 			// Check if this descriptor matches with a word from the last signature (a word not already added to the tree)
 			std::map<float, int> fullResults; // Contains results from the kd-tree search and the naive search in new words
@@ -531,7 +523,10 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
 					}
 					else
 					{
-						UWARN("Not enough nearest neighbors found! fullResults=%d (descriptor %d)", fullResults.size(), i);
+						if(!_dataTree.empty())
+						{
+							UWARN("Not enough nearest neighbors found! fullResults=%d (descriptor %d)", fullResults.size(), i);
+						}
 						badDist = true; // Rejected
 					}
 				}
@@ -566,10 +561,9 @@ std::list<int> VWDictionary::addNewWords(const std::list<std::vector<float> > &
 		ULOGGER_DEBUG("Naive NN");
 		UTimer timer;
 		timer.start();
-		std::list<std::vector<float> >::const_iterator itDesc = descriptors.begin();
-		for(; itDesc!=descriptors.end();++itDesc)
+		for(int i=0; i<descriptors.rows; ++i)
 		{
-			const float* d = itDesc->data();
+			const float* d = descriptors.ptr<float>(i);
 
 			std::map<float, int> results;
 			naiveNNSearch(uValuesList(_visualWords), d, _dim, results, k);
@@ -871,14 +865,14 @@ void VWDictionary::addWord(VisualWord * vw)
 }
 
 // dist = (euclidean dist)^2, "k" nearest neighbors
-void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const float * d, unsigned int length, std::map<float, int> & results, unsigned int k) const
+void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const
 {
 	double total_cost = 0;
 	double t0, t1, t2, t3;
 	const float * dw = 0;
 	bool goodMatch;
 
-	if(!words.size() && k > 0)
+	if(!words.size() || k == 0)
 	{
 		return;
 	}
@@ -896,7 +890,7 @@ void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const fl
 			total_cost += t0*t0;
 
 			// compare descriptors
-			unsigned int i = 0;
+			int i = 0;
 			if(length>=4)
 			{
 				for(; i <= length-4; i += 4 )
@@ -1036,16 +1030,12 @@ void VWDictionary::getCommonWords(unsigned int nbCommonWords, int totalSign, std
 
 const VisualWord * VWDictionary::getWord(int id) const
 {
-	return uValue(_visualWords, id);
+	return uValue(_visualWords, id, (VisualWord *)0);
 }
 
-void VWDictionary::setWordSaved(int id, bool saved)
+const VisualWord * VWDictionary::getUnusedWord(int id) const
 {
-	VisualWord * w = uValue(_visualWords, id);
-	if(w)
-	{
-		w->setSaved(saved);
-	}
+	return uValue(_unusedWords, id, (VisualWord *)0);
 }
 
 std::vector<VisualWord*> VWDictionary::getUnusedWords() const

corelib/src/VWDictionary.h

@@ -50,18 +50,17 @@ public:
 	virtual void update();
 
 	virtual std::list<int> addNewWords(
-			const std::list<std::vector<float> > & descriptors,
-			unsigned int dim,
+			const cv::Mat & descriptors,
 			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 naiveNNSearch(const std::list<VisualWord *> & words, const float * d, 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);
+	const VisualWord * getUnusedWord(int id) const;
 	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;}
@@ -105,12 +104,12 @@ private:
 	float _nndrRatio;
 	unsigned int _maxLeafs;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
-	unsigned int _dim;
+	int _dim;
 	int _lastWordId;
 	NearestNeighbor * _nn;
 	cv::Mat _dataTree;
 	std::map<int ,int> _mapIndexId;
-	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>
+	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
 };
 
 } // namespace rtabmap

corelib/src/VerifyHypotheses.cpp

@@ -19,7 +19,7 @@
 
 #include "VerifyHypotheses.h"
 #include "rtabmap/core/Parameters.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include <cstdlib>
 #include <opencv2/calib3d/calib3d.hpp>
 

corelib/src/VisualWord.cpp

@@ -24,7 +24,7 @@
 namespace rtabmap
 {
 
-VisualWord::VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId) :
+VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureId) :
 	_id(id),
 	_saved(false),
 	_totalReferences(0)

corelib/src/VisualWord.h

@@ -31,7 +31,7 @@ class SignatureSurf;
 class RTABMAP_EXP VisualWord
 {
 public:
-	VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId = 0);
+	VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
 	~VisualWord();
 
 	void addRef(int signatureId);
@@ -40,7 +40,7 @@ public:
 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}
 	const float * getDescriptor() const {return _descriptor;}
-	unsigned int getDim() const {return _dim;}
+	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;}
@@ -49,7 +49,7 @@ public:
 private:
 	int _id;
 	float * _descriptor;
-	unsigned int _dim;
+	int _dim;
 	bool _saved; // If it's saved to bd
 
 	int _totalReferences;

corelib/src/resources/DatabaseSchema.sql

@@ -23,14 +23,30 @@ CREATE TABLE Signature (
 	id INTEGER NOT NULL,
 	type VARCHAR NOT NULL,
 	weight INTEGER,
-	loopClosureId INTEGER,
-	image BLOB,
-	imgWidth INTEGER,
-	imgHeight INTEGER,
+	loopClosureIds BLOB,
+	childLoopClosureIds BLOB,
 	timeEnter DATE,
 	PRIMARY KEY (id),
-	FOREIGN KEY (type) REFERENCES SignatureType(type),
-	FOREIGN KEY (loopClosureId) REFERENCES Signature(id)
+	FOREIGN KEY (type) REFERENCES SignatureType(type)
+);
+
+CREATE TABLE Image (
+	id INTEGER NOT NULL,
+	width INTEGER NOT NULL,
+	height INTEGER NOT NULL,
+	channels INTEGER NOT NULL,
+	compressed CHAR NOT NULL,
+	data BLOB,
+	timeEnter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE SMState (
+	id INTEGER NOT NULL,
+	sensors BLOB,
+	motionMask BLOB,
+	timeEnter DATE,
+	FOREIGN KEY (id) REFERENCES Signature(id)
 );
 
 CREATE TABLE Neighbor (
@@ -38,8 +54,7 @@ CREATE TABLE Neighbor (
     nid INTEGER NOT NULL,
     actionSize INTEGER,
     actions BLOB,
-    timeEnter DATE,
-    PRIMARY KEY (sid, nid),
+    baseIds BLOB,
     FOREIGN KEY (sid) REFERENCES Signature(id),
     FOREIGN KEY (nid) REFERENCES Signature(id)
 );
@@ -66,7 +81,6 @@ CREATE TABLE Map_SS_VW (
 	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)
 );
@@ -90,20 +104,38 @@ CREATE TABLE StatisticsAfterRunSurf (
 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');
+ SELECT RAISE(ABORT, 'Foreign key Signature.type constraint failed');
 END;
 
-CREATE TRIGGER insert_Neighbor BEFORE INSERT ON Neighbor 
+CREATE TRIGGER insert_SMState BEFORE INSERT ON SMState 
+WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key SMState.id constraint failed');
+END;
+
+--CREATE TRIGGER insert_Neighbor_unique BEFORE INSERT ON Neighbor 
+--WHEN NEW.sid = NEW.nid
+--BEGIN
+-- SELECT RAISE(ABORT, 'Cannot add self references');
+--END;
+
+CREATE TRIGGER insert_Neighbor_sid BEFORE INSERT ON Neighbor 
 WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.sid)
 BEGIN
- SELECT RAISE(ABORT, 'Foreign key constraint failed');
+ SELECT RAISE(ABORT, 'Foreign key Neighbor.sid constraint failed');
 END;
 
+--Commented before a link can be added before the neighbor is saved...
+--CREATE TRIGGER insert_Neighbor_nid BEFORE INSERT ON Neighbor 
+--WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.nid)
+--BEGIN
+-- SELECT RAISE(ABORT, 'Foreign key Neighbor.nid 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)
+WHEN NOT EXISTS (SELECT type FROM Signature WHERE Signature.id = NEW.signatureId AND type='KeypointSignature')
 BEGIN
- SELECT RAISE(ABORT, 'Foreign key constraint failed');
+ SELECT RAISE(ABORT, 'KeypointSignature type constraint failed');
 END;
 
  --   Creating a trigger for timeEnter
@@ -112,21 +144,11 @@ 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;
@@ -142,18 +164,19 @@ 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_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_SMState_Id on SMState (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');
+INSERT INTO SignatureType(type) VALUES ('KeypointSignature');
+INSERT INTO SignatureType(type) VALUES ('SMSignature');
 
 -- *******************************************************************
 -- TESTS

corelib/tests/CMakeLists.txt

@@ -9,13 +9,13 @@ SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}/../include
     ${CMAKE_CURRENT_SOURCE_DIR}/../src
     ${CPPUNIT_INCLUDE_DIR}
-    ${UTILITE_INCLUDE_DIR}
+    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 	${SQLITE3_INCLUDE_DIR}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARY}
+    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBRARIES} 
 	${CPPUNIT_LIBRARY}
 	${SQLITE3_LIBRARY}

corelib/tests/Tests.cpp

@@ -22,7 +22,7 @@
 
 //Headers for the test BEGIN
 #include "rtabmap/core/Camera.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "VWDictionary.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/Rtabmap.h"
@@ -76,6 +76,7 @@ void Tests::testAvpd()
 	parameters.insert(ParametersPair(Parameters::kRtabmapPublishStats(), "true"));
 	parameters.insert(ParametersPair(Parameters::kRtabmapTimeThr(), "0"));
 	parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "500"));
+	UDirectory::makeDir("./LogTestAvpdCore/");
 	ctabmap.setWorkingDirectory("./LogTestAvpdCore/");
 	ctabmap.init(parameters);
 
@@ -83,14 +84,14 @@ void Tests::testAvpd()
 	/* Start thread's task */
 	SMState * smState = 0;
 
-	smState = camera.takeImage();
+	smState = camera.takeSMState();
 	int imgCount = 0;
 	while(smState)
 	{
 		++imgCount;
 		printf("Processing image %d/84...\n", imgCount);
 		ctabmap.process(smState);
-		smState = camera.takeImage();
+		smState = camera.takeSMState();
 	}
 	if(smState)
 	{
@@ -140,7 +141,6 @@ void Tests::testAvpd()
 
 void Tests::testCamera()
 {
-	//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testCamera.txt", false);
 	std::string path;
 	SMState * smState = 0;
 	int count;
@@ -169,33 +169,33 @@ void Tests::testCamera()
 
 	//CameraImages class
 	path = "data/samples";
-	CameraImages cameraImages(path, false, 0, false, 80);
+	CameraImages cameraImages(path, 1, false, 0, false, 80);
 	CPPUNIT_ASSERT( cameraImages.init() );
 	CPPUNIT_ASSERT( cameraImages.isIdle() == true);
-	smState = cameraImages.takeImage();
+	smState = cameraImages.takeSMState();
 	count = 0;
 	while(smState)
 	{
 		delete smState;
 		smState = 0;
 		++count;
-		smState = cameraImages.takeImage();
+		smState = cameraImages.takeSMState();
 	}
-	CPPUNIT_ASSERT( count == 5 );
+	CPPUNIT_ASSERT( count == 84 );
 
 	//CameraDatabase class
 	path = "./data/samples.db";
-	CameraDatabase cameraDatabase(path, false); // ignoreChildren=false;
+	CameraDatabase cameraDatabase(path, false, false); // ignoreChildren=false; loadActions=false
 	CPPUNIT_ASSERT( cameraDatabase.init() );
 	CPPUNIT_ASSERT( cameraDatabase.isIdle() == true);
-	smState = cameraDatabase.takeImage();
+	smState = cameraDatabase.takeSMState();
 	count = 0;
 	while(smState)
 	{
 		++count;
 		delete smState;
 		smState = 0;
-		smState = cameraDatabase.takeImage();
+		smState = cameraDatabase.takeSMState();
 	}
 	//ULOGGER_INFO("%d", count);
 	CPPUNIT_ASSERT( count == 82 );
@@ -217,29 +217,84 @@ void Tests::testDBDriverFactory()
 // 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");
-
+	CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
 	delete driver;
 
-	ULogger::write("testSqlite3Database end...");
+	//== Test SMSignature save/load ==//
+	UFile::erase("LogTestAvpdCore/tmpDatabase.db");
+	parameters.clear();
+	parameters.insert(ParametersPair(Parameters::kMemSignatureType(), "1"));
+	driver = DBDriverFactory::createDBDriver("sqlite3", parameters);
+	CPPUNIT_ASSERT(driver);
+	CPPUNIT_ASSERT(driver->openConnection("LogTestAvpdCore/tmpDatabase.db"));
+	std::vector<int> sensors;
+	std::vector<unsigned char> motionMask;
+	std::vector<int> sensors1;
+	std::vector<int> sensors2;
+	bool keepRawData = true;
+	CameraImages camera("./data/samples");
+	CPPUNIT_ASSERT_MESSAGE("Camera initialization failed!\n", camera.init());
+
+	IplImage * image = camera.takeImage();
+	CPPUNIT_ASSERT(image);
+	SMSignature * sm1 = new SMSignature(sensors, motionMask, 1, image, keepRawData);
+	sensors1 = sm1->getSensors();
+	cvReleaseImage(&image);
+	sm1->addNeighbor(NeighborLink(2));
+
+	image = camera.takeImage();
+	CPPUNIT_ASSERT(image);
+	SMSignature * sm2 = new SMSignature(sensors, motionMask, 2, image, keepRawData);
+	sensors2 = sm2->getSensors();
+	cvReleaseImage(&image);
+	sm2->addNeighbor(NeighborLink(1));
+
+	//save them
+	driver->asyncSave(sm1);
+	sm1 = 0;
+	driver->asyncSave(sm2);
+	sm2 = 0;
+	driver->emptyTrashes(false);
+
+	//load them
+	Signature * s1 = 0;
+	Signature * s2 = 0;
+	driver->getSignature(1, &s1);
+	driver->getSignature(2, &s2);
+	CPPUNIT_ASSERT(s1);
+	CPPUNIT_ASSERT(s2);
+	sm1 = dynamic_cast<SMSignature*>(s1);
+	sm2 = dynamic_cast<SMSignature*>(s2);
+	CPPUNIT_ASSERT(sm1);
+	CPPUNIT_ASSERT(sm2);
+
+	//compare
+	CPPUNIT_ASSERT(sensors1.size() > 0 && sensors1.size() == sm1->getSensors().size());
+	CPPUNIT_ASSERT(sensors2.size() > 0 && sensors2.size() == sm2->getSensors().size());
+	for(unsigned int i=0; i<sensors1.size(); ++i)
+	{
+		CPPUNIT_ASSERT(sensors1.at(i) == sm1->getSensors().at(i));
+	}
+	for(unsigned int i=0; i<sensors2.size(); ++i)
+	{
+		CPPUNIT_ASSERT(sensors2.at(i) == sm2->getSensors().at(i));
+	}
+	CPPUNIT_ASSERT(sm1->getNeighbors().size() == 1);
+	CPPUNIT_ASSERT(sm2->getNeighbors().size() == 1);
+
+	delete sm1;
+	delete sm2;
+	delete driver;
 }
 
 // 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
@@ -250,14 +305,14 @@ void Tests::testBayesFilter()
 	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( 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 );
+	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");
@@ -265,7 +320,7 @@ void Tests::testBayesFilter()
 	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 );
+	CPPUNIT_ASSERT( bayes.getPredictionLCStr().compare("0.01 0.02 0.03") == 0 );
 
 	//reset parameters...
 	bayes = BayesFilter();
@@ -277,7 +332,7 @@ void Tests::testBayesFilter()
 	//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.setPredictionLC("0.1 0.24 0.18 0.1 0.04 0.01");
 	bayes.setVirtualPlacePrior(0.9);
 
 	std::map<int, float> likelihood;
@@ -307,7 +362,7 @@ void Tests::testBayesFilter()
 		}
 	}
 	// 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};
+	int resultWanted1[100] = {1000,0,0,0,0,0,0,0,0,0,900,99,0,0,0,0,0,0,0,0,820,117,62,0,0,0,0,0,0,0,756,111,82,50,0,0,0,0,0,0,704,103,84,67,40,0,0,0,0,0,663,96,82,69,54,32,0,0,0,0,631,90,79,69,58,44,26,0,0,0,604,84,76,68,58,48,36,21,0,0,583,79,73,66,58,49,40,30,17,0,567,74,69,64,58,50,41,33,25,14};
 	for(int i=0; i<100; ++i)
 	{
 		ULOGGER_DEBUG("%d vs %d", result[i], resultWanted1[i]);
@@ -361,7 +416,8 @@ void Tests::testKeypointMemory()
 		CPPUNIT_ASSERT(*i == signWordsRequired[j]);
 		++j;
 	}
-	likelihood = mem.computeLikelihood(lastSign);
+	float maximumScore;
+	likelihood = mem.computeLikelihood(lastSign, std::list<int>(mem.getWorkingMem().begin(), mem.getWorkingMem().end()), maximumScore);
 	//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};
@@ -378,16 +434,17 @@ void Tests::testVWDictionary()
 	VWDictionary dictionary;
 	dictionary.setNndrUsed(false);
 	std::list<cv::KeyPoint> keypoints;
-	std::list<std::vector<float> > descriptors;
+	cv::Mat 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 = cv::Mat(0, 2, CV_32F);
 	descriptors.push_back(v);
 
-	dictionary.addNewWords(descriptors, dim, 1);
+	dictionary.addNewWords(descriptors, 1);
 	CPPUNIT_ASSERT(dictionary.getVisualWords().size() == 1);
 
 	//Create a word with the next descriptor (the distance^2 with the first word added = 2)