Included UtiLite library directly in Rtabmap source (to simplify the installation)

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

CMakeLists.txt

@@ -117,7 +117,6 @@ IF(APPLE)
 ENDIF(APPLE)
 
 ####### DEPENDENCIES #######
-FIND_PACKAGE(UtiLite REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(Sqlite3 REQUIRED)
 
@@ -166,6 +165,7 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 
 ####### SOURCES (Projects) #######
+ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
 
 IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)

app/src/CMakeLists.txt

@@ -13,17 +13,16 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 )
 
 INCLUDE(${QT_USE_FILE})
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
     ${QT_LIBRARIES} 
 	${OpenCV_LIBS} 
 )
@@ -60,7 +59,7 @@ IF(APPLE AND BUILD_AS_BUNDLE)
 ELSE()
   ADD_EXECUTABLE(main_app WIN32 ${SRC_FILES})
 ENDIF()
-TARGET_LINK_LIBRARIES(main_app rtabmap_corelib rtabmap_guilib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(main_app rtabmap_corelib rtabmap_guilib rtabmap_utilite ${LIBRARIES})
 
 IF(APPLE AND BUILD_AS_BUNDLE)
   SET_TARGET_PROPERTIES(main_app PROPERTIES

app/src/ObjDeletionHandler.h

@@ -20,8 +20,8 @@
 #ifndef OBJDELETIONHANDLER_H_
 #define OBJDELETIONHANDLER_H_
 
-#include "utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsHandler.h"
-#include "utilite/UEvent.h"
+#include "rtabmap/utilite/UEvent.h"
 #include <QtCore/QObject>
 
 class ObjDeletionHandler : public QObject, public UEventsHandler

app/src/main.cpp

@@ -19,11 +19,11 @@
 
 #include <QtGui/QApplication>
 #include <QtCore/QDir>
-#include "utilite/UEventsManager.h"
+#include "rtabmap/utilite/UEventsManager.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/gui/MainWindow.h"
 #include <QtGui/QMessageBox>
-#include "utilite/UObjDeletionThread.h"
+#include "rtabmap/utilite/UObjDeletionThread.h"
 #include "ObjDeletionHandler.h"
 
 using namespace rtabmap;

cmake_modules/FindPthreads.cmake

@@ -0,0 +1,98 @@
+# - Find the Pthreads library
+# This module searches for the Pthreads library (including the
+# pthreads-win32 port).
+#
+# This module defines these variables:
+#
+#  PTHREADS_FOUND
+#      True if the Pthreads library was found
+#  PTHREADS_LIBRARY
+#      The location of the Pthreads library
+#  PTHREADS_INCLUDE_DIR
+#      The include path of the Pthreads library
+#  PTHREADS_DEFINITIONS
+#      Preprocessor definitions to define
+#
+# This module responds to the PTHREADS_EXCEPTION_SCHEME
+# variable on Win32 to allow the user to control the
+# library linked against.  The Pthreads-win32 port
+# provides the ability to link against a version of the
+# library with exception handling.  IT IS NOT RECOMMENDED
+# THAT YOU USE THIS because most POSIX thread implementations
+# do not support stack unwinding.
+#
+#  PTHREADS_EXCEPTION_SCHEME
+#      C  = no exceptions (default)
+#         (NOTE: This is the default scheme on most POSIX thread
+#          implementations and what you should probably be using)
+#      CE = C++ Exception Handling
+#      SE = Structure Exception Handling (MSVC only)
+#
+
+#
+# Define a default exception scheme to link against
+# and validate user choice.
+#
+IF(NOT DEFINED PTHREADS_EXCEPTION_SCHEME)
+    # Assign default if needed
+    SET(PTHREADS_EXCEPTION_SCHEME "C")
+ELSE(NOT DEFINED PTHREADS_EXCEPTION_SCHEME)
+    # Validate
+    IF(NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "C" AND
+       NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "CE" AND
+       NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "SE")
+
+    MESSAGE(FATAL_ERROR "See documentation for FindPthreads.cmake, only C, CE, and SE modes are allowed")
+
+    ENDIF(NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "C" AND
+          NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "CE" AND
+          NOT PTHREADS_EXCEPTION_SCHEME STREQUAL "SE")
+
+     IF(NOT MSVC AND PTHREADS_EXCEPTION_SCHEME STREQUAL "SE")
+         MESSAGE(FATAL_ERROR "Structured Exception Handling is only allowed for MSVC")
+     ENDIF(NOT MSVC AND PTHREADS_EXCEPTION_SCHEME STREQUAL "SE")
+
+ENDIF(NOT DEFINED PTHREADS_EXCEPTION_SCHEME)
+
+#
+# Find the header file
+#
+FIND_PATH(PTHREADS_INCLUDE_DIR pthread.h)
+
+#
+# Find the library
+#
+SET(names)
+IF(MSVC)
+    SET(names
+            pthreadV${PTHREADS_EXCEPTION_SCHEME}2
+            pthread
+    )
+ELSEIF(MINGW)
+    SET(names
+            pthreadG${PTHREADS_EXCEPTION_SCHEME}2
+            pthread
+    )
+ELSE(MSVC) # Unix / Cygwin / Apple
+    SET(names pthread)
+ENDIF(MSVC)
+    
+FIND_LIBRARY(PTHREADS_LIBRARY ${names}
+    DOC "The Portable Threads Library")
+
+IF(PTHREADS_INCLUDE_DIR AND PTHREADS_LIBRARY)
+    SET(PTHREADS_FOUND true)
+    SET(PTHREADS_DEFINITIONS -DHAVE_PTHREAD_H)
+    SET(PTHREADS_INCLUDE_DIRS ${PTHREADS_INCLUDE_DIR})
+    SET(PTHREADS_LIBRARIES    ${PTHREADS_LIBRARY})
+ENDIF(PTHREADS_INCLUDE_DIR AND PTHREADS_LIBRARY)
+
+IF(PTHREADS_FOUND)
+    IF(NOT PTHREADS_FIND_QUIETLY)
+        MESSAGE(STATUS "Found Pthreads")
+    ENDIF(NOT PTHREADS_FIND_QUIETLY)
+ELSE(PTHREADS_FOUND) 
+    IF(PTHREADS_FIND_REQUIRED)
+        MESSAGE(FATAL_ERROR "Could not find the Pthreads library")
+    ENDIF(PTHREADS_FIND_REQUIRED)
+ENDIF(PTHREADS_FOUND)

cmake_modules/FindUtiLite.cmake

@@ -1,130 +0,0 @@
-# - Find UtiLite
-# This module finds an installed UtiLite package.
-#
-# It sets the following variables:
-#  UtiLite_FOUND              - Set to false, or undefined, if UtiLite isn't found.
-#  UtiLite_INCLUDE_DIRS        - The UtiLite include directory.
-#  UtiLite_LIBRARIES            - The UtiLite library to link against.
-#  URESOURCEGENERATOR_EXEC    - The resource generator tool executable
-#
-# Backward compatibility:
-#  UTILITE_FOUND              - Set to false, or undefined, if UtiLite isn't found.
-#  UTILITE_INCLUDE_DIRS       - The UtiLite include directory.
-#  UTILITE_LIBRARIES          - The UtiLite library to link against.
-#  UTILITE_INCLUDE_DIR        - The UtiLite include directory.
-#  UTILITE_LIBRARY            - The UtiLite library to link against.
-#
-# 
-
-SET(UtiLite_VERSION_REQUIRED 0.3.0)
-
-SET(UtiLite_ROOT)
-
-# Add ROS UtiLite directory if ROS is installed
-FIND_PROGRAM(ROSPACK_EXEC NAME rospack PATHS)  
-IF(ROSPACK_EXEC)  
-	EXECUTE_PROCESS(COMMAND ${ROSPACK_EXEC} find utilite 
-			   	    OUTPUT_VARIABLE UtiLite_ROS_PATH
-					OUTPUT_STRIP_TRAILING_WHITESPACE
-					WORKING_DIRECTORY "./"
-	)
-	IF(UtiLite_ROS_PATH)
-	    MESSAGE(STATUS "Found UtiLite ROS pkg : ${UtiLite_ROS_PATH}")
-	    SET(UtiLite_ROOT
-	        ${UtiLite_ROS_PATH}/utilite
-	        ${UtiLite_ROOT}
-	    )
-	ENDIF(UtiLite_ROS_PATH)
-ENDIF(ROSPACK_EXEC)
-
-FIND_PROGRAM(URESOURCEGENERATOR_EXEC NAME uresourcegenerator PATHS ${UtiLite_ROOT}/bin)  
-IF(URESOURCEGENERATOR_EXEC)  
-	EXECUTE_PROCESS(COMMAND ${URESOURCEGENERATOR_EXEC} -v 
-			   	    OUTPUT_VARIABLE UtiLite_VERSION
-					OUTPUT_STRIP_TRAILING_WHITESPACE
-					WORKING_DIRECTORY "./"
-	)
-	
-	IF(UtiLite_VERSION VERSION_LESS UtiLite_VERSION_REQUIRED)
-	    IF(UtiLite_FIND_REQUIRED)
-	    	MESSAGE(FATAL_ERROR "Your version of UtiLite is too old (${UtiLite_VERSION}), UtiLite ${UtiLite_VERSION_REQUIRED} is required.")
-	    ENDIF(UtiLite_FIND_REQUIRED)
-	ENDIF(UtiLite_VERSION VERSION_LESS UtiLite_VERSION_REQUIRED)
-
-	IF(WIN32)
-		FIND_PATH(UtiLite_INCLUDE_DIRS 
-				utilite/UEventsManager.h
-				PATH_SUFFIXES "../include")
-	
-		FIND_LIBRARY(UtiLite_LIBRARIES 
-				NAMES utilite
-			 	PATH_SUFFIXES "../lib")
-		FIND_LIBRARY(UtiLite_Qt 
-				NAMES utilite_qt
-			 	PATH_SUFFIXES "../lib")
-		FIND_LIBRARY(UtiLite_Audio 
-				NAMES utilite_audio
-			 	PATH_SUFFIXES "../lib")
-		FIND_LIBRARY(UtiLite_Cv 
-				NAMES utilite_cv
-			 	PATH_SUFFIXES "../lib")
-		FIND_LIBRARY(UtiLite_Cvqt 
-				NAMES utilite_cvqt
-			 	PATH_SUFFIXES "../lib")
-	
-	ELSE()
-		FIND_PATH(UtiLite_INCLUDE_DIRS 
-				utilite/UEventsManager.h
-				PATHS ${UtiLite_ROOT}/include)
-	
-		FIND_LIBRARY(UtiLite_LIBRARIES 
-				NAMES utilite
-				PATHS ${UtiLite_ROOT}/lib)
-		FIND_LIBRARY(UtiLite_Qt 
-				NAMES utilite_qt
-				PATHS ${UtiLite_ROOT}/lib)
-		FIND_LIBRARY(UtiLite_Audio 
-				NAMES utilite_audio
-				PATHS ${UtiLite_ROOT}/lib)
-		FIND_LIBRARY(UtiLite_Cv 
-				NAMES utilite_cv
-				PATHS ${UtiLite_ROOT}/lib)
-		FIND_LIBRARY(UtiLite_Cvqt 
-				NAMES utilite_cvqt
-				PATHS ${UtiLite_ROOT}/lib)
-	ENDIF()
-	IF(UtiLite_LIBRARIES AND UtiLite_Qt)
-		SET(UtiLite_LIBRARIES ${UtiLite_LIBRARIES} ${UtiLite_Qt})
-	ENDIF(UtiLite_LIBRARIES AND UtiLite_Qt)
-	IF(UtiLite_LIBRARIES AND UtiLite_Audio)
-		SET(UtiLite_LIBRARIES ${UtiLite_LIBRARIES} ${UtiLite_Audio})
-	ENDIF(UtiLite_LIBRARIES AND UtiLite_Audio)
-	IF(UtiLite_LIBRARIES AND UtiLite_Cv)
-		SET(UtiLite_LIBRARIES ${UtiLite_LIBRARIES} ${UtiLite_Cv})
-	ENDIF(UtiLite_LIBRARIES AND UtiLite_Cv)
-	IF(UtiLite_LIBRARIES AND UtiLite_Cvqt)
-		SET(UtiLite_LIBRARIES ${UtiLite_LIBRARIES} ${UtiLite_Cvqt})
-	ENDIF(UtiLite_LIBRARIES AND UtiLite_Cvqt)
-	
-	IF (UtiLite_INCLUDE_DIRS AND UtiLite_LIBRARIES)
-	   SET(UtiLite_FOUND TRUE)
-	ENDIF (UtiLite_INCLUDE_DIRS AND UtiLite_LIBRARIES)
-ENDIF(URESOURCEGENERATOR_EXEC)
-
-IF (UtiLite_FOUND)
-   # show which UtiLite was found only if not quiet
-   IF (NOT UtiLite_FIND_QUIETLY)
-      MESSAGE(STATUS "Found UtiLite ${UtiLite_VERSION}")
-   ENDIF (NOT UtiLite_FIND_QUIETLY)
-   # backward compatibility...
-   SET(UTILITE_FOUND ${UtiLite_FOUND})
-   SET(UTILITE_INCLUDE_DIRS ${UtiLite_INCLUDE_DIRS})
-   SET(UTILITE_LIBRARIES ${UtiLite_LIBRARIES})
-   SET(UTILITE_INCLUDE_DIR ${UtiLite_INCLUDE_DIRS})
-   SET(UTILITE_LIBRARY ${UtiLite_LIBRARIES})
-ELSE ()
-   # fatal error if UtiLite is required but not found
-   IF (UtiLite_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find UtiLite. Verify your PATH if it is already installed or download it at http://utilite.googlecode.com")
-   ENDIF (UtiLite_FIND_REQUIRED)
-ENDIF ()

corelib/include/rtabmap/core/CameraEvent.h

@@ -21,7 +21,7 @@
 
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/features2d/features2d.hpp>
-#include <utilite/UEvent.h>
+#include <rtabmap/utilite/UEvent.h>
 #include "rtabmap/core/Image.h"
 
 namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -21,8 +21,8 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <utilite/UThreadNode.h>
+#include <rtabmap/utilite/UThreadNode.h>
-#include <utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsHandler.h>
 
 #include "rtabmap/core/Parameters.h"
 

corelib/include/rtabmap/core/DBDriver.h

@@ -27,8 +27,8 @@
 #include <map>
 #include <set>
 #include <opencv2/core/core.hpp>
-#include "utilite/UMutex.h"
+#include "rtabmap/utilite/UMutex.h"
-#include "utilite/UThreadNode.h"
+#include "rtabmap/utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"
 
 namespace rtabmap {

corelib/include/rtabmap/core/DBReader.h

@@ -10,8 +10,8 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <utilite/UThreadNode.h>
+#include <rtabmap/utilite/UThreadNode.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
 
 #include <opencv2/core/core.hpp>
 

corelib/include/rtabmap/core/Memory.h

@@ -22,14 +22,14 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include "utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/Image.h"
 #include <typeinfo>
 #include <list>
 #include <map>
 #include <set>
-#include "utilite/UStl.h"
+#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 

corelib/include/rtabmap/core/ParamEvent.h

@@ -21,7 +21,7 @@
 #define PARAMEVENT_H_
 
 #include "rtabmap/core/Parameters.h"
-#include <utilite/UEvent.h>
+#include <rtabmap/utilite/UEvent.h>
 
 namespace rtabmap
 {

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -22,7 +22,7 @@
 
 
 
-#include <utilite/UEvent.h>
+#include <rtabmap/utilite/UEvent.h>
 #include "rtabmap/core/Statistics.h"
 
 namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -22,10 +22,10 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <utilite/UThreadNode.h>
+#include <rtabmap/utilite/UThreadNode.h>
-#include <utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsHandler.h>
-#include <utilite/USemaphore.h>
+#include <rtabmap/utilite/USemaphore.h>
-#include <utilite/UMutex.h>
+#include <rtabmap/utilite/UMutex.h>
 
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Image.h"

corelib/src/BayesFilter.cpp

@@ -23,7 +23,7 @@
 #include "rtabmap/core/Parameters.h"
 #include <iostream>
 
-#include "utilite/UtiLite.h"
+#include "rtabmap/utilite/UtiLite.h"
 
 namespace rtabmap {
 

corelib/src/BayesFilter.h

@@ -25,7 +25,7 @@
 #include <opencv2/core/core.hpp>
 #include <list>
 #include <set>
-#include "utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 
 namespace rtabmap {

corelib/src/CMakeLists.txt

@@ -25,16 +25,15 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+    ${PROJECT_SOURCE_DIR}/utilite/include
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}
 	${CMAKE_CURRENT_BINARY_DIR}
-	${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 	${SQLITE3_INCLUDE_DIR}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBS} 
 	${SQLITE3_LIBRARY}
 )
@@ -57,9 +56,9 @@ SET(RESOURCES_HEADERS
 
 ADD_CUSTOM_COMMAND(
    OUTPUT ${RESOURCES_HEADERS}
-   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
+   COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/uresourcegenerator -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
    COMMENT "[Creating resources]"
-   DEPENDS ${R}
+   DEPENDS ${R} uresourcegenerator
 )
 
 ####################################
@@ -73,7 +72,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES_HEADERS})
-TARGET_LINK_LIBRARIES(rtabmap_corelib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_corelib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES(
 rtabmap_corelib 

corelib/src/Camera.cpp

@@ -21,13 +21,13 @@
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/Features2d.h"
 
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UFile.h>
+#include <rtabmap/utilite/UFile.h>
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
 
 #include <opencv2/imgproc/imgproc.hpp>
 

corelib/src/CameraThread.cpp

@@ -22,8 +22,8 @@
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
 
 namespace rtabmap
 {

corelib/src/DBDriver.cpp

@@ -21,11 +21,11 @@
 
 #include "Signature.h"
 #include "VisualWord.h"
-#include "utilite/UConversion.h"
+#include "rtabmap/utilite/UConversion.h"
-#include "utilite/UMath.h"
+#include "rtabmap/utilite/UMath.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UTimer.h"
+#include "rtabmap/utilite/UTimer.h"
-#include "utilite/UStl.h"
+#include "rtabmap/utilite/UStl.h"
 
 namespace rtabmap {
 

corelib/src/DBDriverSqlite3.cpp

@@ -25,7 +25,7 @@
 #include "DatabaseSchema_sql.h"
 #include <set>
 
-#include "utilite/UtiLite.h"
+#include "rtabmap/utilite/UtiLite.h"
 
 namespace rtabmap {
 

corelib/src/DBReader.cpp

@@ -9,9 +9,9 @@
 #include "rtabmap/core/DBDriver.h"
 #include "DBDriverSqlite3.h"
 
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/UFile.h>
+#include <rtabmap/utilite/UFile.h>
 
 #include "rtabmap/core/CameraEvent.h"
 

corelib/src/EpipolarGeometry.cpp

@@ -19,10 +19,10 @@
 
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "Signature.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UTimer.h"
+#include "rtabmap/utilite/UTimer.h"
-#include "utilite/UStl.h"
+#include "rtabmap/utilite/UStl.h"
-#include "utilite/UMath.h"
+#include "rtabmap/utilite/UMath.h"
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/core_c.h>

corelib/src/Features2d.cpp

@@ -18,12 +18,12 @@
  */
 
 #include "rtabmap/core/Features2d.h"
-#include "utilite/UStl.h"
+#include "rtabmap/utilite/UStl.h"
-#include "utilite/UConversion.h"
+#include "rtabmap/utilite/UConversion.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UMath.h"
+#include "rtabmap/utilite/UMath.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UTimer.h"
+#include "rtabmap/utilite/UTimer.h"
 #include <opencv2/imgproc/imgproc_c.h>
 #include <opencv2/gpu/gpu.hpp>
 #include <opencv2/core/version.hpp>

corelib/src/Memory.cpp

@@ -17,12 +17,12 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UProcessInfo.h>
+#include <rtabmap/utilite/UProcessInfo.h>
-#include <utilite/UMath.h>
+#include <rtabmap/utilite/UMath.h>
 
 #include "rtabmap/core/Memory.h"
 #include "Signature.h"

corelib/src/NearestNeighbor.cpp

@@ -18,7 +18,7 @@
  */
 
 #include "NearestNeighbor.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 #include <opencv2/core/core.hpp>
 
 namespace rtabmap

corelib/src/Parameters.cpp

@@ -18,8 +18,8 @@
  */
 
 #include "rtabmap/core/Parameters.h"
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap
 {

corelib/src/Rtabmap.cpp

@@ -28,11 +28,11 @@
 #include "rtabmap/core/Memory.h"
 #include "BayesFilter.h"
 
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UFile.h>
+#include <rtabmap/utilite/UFile.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UMath.h>
+#include <rtabmap/utilite/UMath.h>
 
 #include "SimpleIni.h"
 
@@ -56,7 +56,7 @@
 
 namespace rtabmap
 {
-const char * Rtabmap::kDefaultDatabaseName = "LTM.db";
+const char * Rtabmap::kDefaultDatabaseName = "rtabmap.db";
 
 Rtabmap::Rtabmap() :
 	_publishStats(Parameters::defaultRtabmapPublishStats()),

corelib/src/RtabmapThread.cpp

@@ -24,9 +24,9 @@
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
 
 namespace rtabmap {
 

corelib/src/Signature.cpp

@@ -22,7 +22,7 @@
 #include "rtabmap/core/Memory.h"
 #include <opencv2/highgui/highgui.hpp>
 
-#include <utilite/UtiLite.h>
+#include <rtabmap/utilite/UtiLite.h>
 
 namespace rtabmap
 {

corelib/src/Statistics.cpp

@@ -18,7 +18,7 @@
  */
 
 #include "rtabmap/core/Statistics.h"
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
 
 namespace rtabmap {
 std::map<std::string, float> Statistics::_defaultData;

corelib/src/VWDictionary.cpp

@@ -25,7 +25,7 @@
 #include "NearestNeighbor.h"
 #include "rtabmap/core/Parameters.h"
 
-#include "utilite/UtiLite.h"
+#include "rtabmap/utilite/UtiLite.h"
 
 #include <fstream>
 #include <string>

corelib/src/VisualWord.cpp

@@ -18,8 +18,8 @@
  */
 
 #include "VisualWord.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UStl.h"
+#include "rtabmap/utilite/UStl.h"
 
 namespace rtabmap
 {

examples/main.cpp

@@ -120,7 +120,7 @@ int main(int argc, char * argv[])
 	printf("Generated graph \"Graph.dot\", viewable with Graphiz using \"neato -Tpdf Graph.dot -o out.pdf\"\n");
 
 	// Cleanup... save database and logs
-	printf("Saving Long-Term Memory to \"LTM.db\"...\n");
+	printf("Saving Long-Term Memory to \"rtabmap.db\"...\n");
 	rtabmap.close();
 
 	return 0;

guilib/include/rtabmap/gui/MainWindow.h

@@ -22,7 +22,7 @@
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include "utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsHandler.h"
 #include <QtGui/QMainWindow>
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"

guilib/src/CMakeLists.txt

@@ -11,6 +11,7 @@ SET(headers_ui
 	./PdfPlot.h
 	./StatsToolBox.h
 	./DetailedProgressDialog.h
+	./utilite/UPlot.h
 )
 
 SET(uis
@@ -50,6 +51,7 @@ SET(SRC_FILES
 	./AboutDialog.cpp
 	./ConsoleWidget.cpp
 	./DatabaseViewer.cpp
+	./utilite/UPlot.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}
@@ -57,9 +59,9 @@ SET(SRC_FILES
 
 SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 	${CMAKE_CURRENT_SOURCE_DIR}
-	${UTILITE_INCLUDE_DIRS}
 	${OpenCV_INCLUDE_DIRS}
 	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
 )
@@ -67,7 +69,6 @@ SET(INCLUDE_DIRS
 INCLUDE(${QT_USE_FILE})
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
     ${QT_LIBRARIES} 
 	${OpenCV_LIBS} 
 )
@@ -78,7 +79,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # create a library from the source files
 ADD_LIBRARY(rtabmap_guilib ${SRC_FILES})
 # Linking with Qt libraries
-TARGET_LINK_LIBRARIES(rtabmap_guilib rtabmap_corelib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_guilib rtabmap_corelib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES(
 rtabmap_guilib 

guilib/src/ConsoleWidget.cpp

@@ -19,8 +19,8 @@
 
 #include "ConsoleWidget.h"
 #include "ui_consoleWidget.h"
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
 #include <QtGui/QMessageBox>
 #include <QtGui/QTextCursor>
 #include <QtCore/QTimer>

guilib/src/ConsoleWidget.h

@@ -20,7 +20,7 @@
 #ifndef CONSOLEWIDGET_H_
 #define CONSOLEWIDGET_H_
 
-#include <utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsHandler.h>
 #include <QtGui/QWidget>
 #include <QtCore/QMutex>
 #include <QtCore/QTimer>

guilib/src/DatabaseViewer.cpp

@@ -24,11 +24,11 @@
 #include <QtGui/QInputDialog>
 #include <QtCore/QBuffer>
 #include <QtCore/QTextStream>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core/core_c.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/gui/KeypointItem.h"

guilib/src/DetailedProgressDialog.cpp

@@ -26,7 +26,7 @@
 #include <QtGui/QCloseEvent>
 #include <QtGui/QCheckBox>
 #include <QtCore/QTimer>
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap {
 

guilib/src/ImageView.cpp

@@ -25,7 +25,7 @@
 #include <QtGui/QFileDialog>
 #include <QtCore/QDir>
 #include <QtGui/QAction>
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 
 namespace rtabmap {

guilib/src/KeypointItem.cpp

@@ -22,7 +22,7 @@
 #include <QtGui/QPen>
 #include <QtGui/QBrush>
 #include <QtGui/QGraphicsScene>
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap {
 

guilib/src/MainWindow.cpp

@@ -32,11 +32,11 @@
 #include "rtabmap/gui/qtipl.h"
 #include "rtabmap/gui/KeypointItem.h"
 
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UPlot.h>
+#include "utilite/UPlot.h"
-#include <utilite/UEventsManager.h>
+#include <rtabmap/utilite/UEventsManager.h>
-#include <utilite/UFile.h>
+#include <rtabmap/utilite/UFile.h>
 
 #include "AboutDialog.h"
 #include "PdfPlot.h"
@@ -451,8 +451,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
 
 		// get images
-		cv::Mat refImage = stat.refImage();
+		const cv::Mat & refImage = stat.refImage();
-		cv::Mat loopImage = stat.loopImage();
+		const cv::Mat & loopImage = stat.loopImage();
 
 		_ui->label_matchId->clear();
 		QPixmap refPixmap;
@@ -1325,7 +1325,7 @@ void MainWindow::generateLocalMap()
 void MainWindow::deleteMemory()
 {
 	QMessageBox::StandardButton button;
-	QString dbPath = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "LTM.db";
+	QString dbPath = _preferencesDialog->getWorkingDirectory() + QDir::separator() + "rtabmap.db";
 	if(_state == kMonitoring)
 	{
 		button = QMessageBox::question(this,
@@ -1385,7 +1385,7 @@ void MainWindow::updateEditMenu()
 	// Update Memory delete database size
 	if(_state != kMonitoring)
 	{
-		QString dbPath = _preferencesDialog->getWorkingDirectory() + "/LTM.db";
+		QString dbPath = _preferencesDialog->getWorkingDirectory() + "/rtabmap.db";
 		_ui->actionDelete_memory->setText(tr("Delete memory (%1 MB)").arg(UFile::length(dbPath.toStdString())/1000000));
 	}
 }

guilib/src/PdfPlot.cpp

@@ -18,7 +18,7 @@
  */
 
 #include "PdfPlot.h"
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap {
 

guilib/src/PreferencesDialog.cpp

@@ -31,10 +31,10 @@
 #include "ui_preferencesDialog.h"
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Parameters.h"
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UPlot.h>
+#include "utilite/UPlot.h"
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
 
 #define DEFAULT_GUI_IMAGES_KEPT true
 #define DEFAULT_LOGGER_LEVEL 2

guilib/src/StatsToolBox.cpp

@@ -31,8 +31,8 @@
 #include <QtGui/QToolBox>
 #include <QtGui/QDialog>
 
-#include <utilite/UPlot.h>
+#include "utilite/UPlot.h"
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtabmap {
 

guilib/src/qtipl.cpp

@@ -18,7 +18,7 @@
  */
 
 #include "rtabmap/gui/qtipl.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 #include <opencv2/core/core_c.h>
 
 namespace rtabmap {

tools/ConsoleApp/CMakeLists.txt

@@ -4,15 +4,14 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 	${CMAKE_CURRENT_SOURCE_DIR}/../include
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBRARIES} 
 )
 
@@ -22,7 +21,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # Add binary called "consoleApp" that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_EXECUTABLE(consoleApp ${SRC_FILES})
-TARGET_LINK_LIBRARIES(consoleApp rtabmap_corelib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(consoleApp rtabmap_corelib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( consoleApp 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-console)

tools/ConsoleApp/main.cpp

@@ -17,14 +17,14 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/Camera.h"
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
-#include <utilite/UFile.h>
+#include <rtabmap/utilite/UFile.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
 #include <fstream>
 #include <queue>
 #include <opencv2/core/core.hpp>

tools/EpipolarGeometry/CMakeLists.txt

@@ -4,6 +4,7 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${UTILITE_INCLUDE_DIRS}
@@ -21,7 +22,7 @@ SET(LIBRARIES
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(epipolar_geometry WIN32 ${SRC_FILES})
-TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_corelib rtabmap_guilib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(epipolar_geometry rtabmap_corelib rtabmap_guilib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( epipolar_geometry 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-epipolar_geometry)

tools/EpipolarGeometry/main.cpp

@@ -3,12 +3,12 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <iostream>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
-#include <utilite/UMath.h>
+#include <rtabmap/utilite/UMath.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include "rtabmap/core/Features2d.h"
 #include "rtabmap/core/EpipolarGeometry.h"

tools/ImagesDbExtractor/CMakeLists.txt

@@ -4,6 +4,7 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${UTILITE_INCLUDE_DIRS}
@@ -18,7 +19,7 @@ SET(LIBRARIES
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(imagesDbExtractor ${SRC_FILES})
-TARGET_LINK_LIBRARIES(imagesDbExtractor rtabmap_corelib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(imagesDbExtractor rtabmap_corelib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( imagesDbExtractor 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesDbExtractor)

tools/ImagesDbExtractor/main.cpp

@@ -3,10 +3,10 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <iostream>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UDirectory.h>
+#include <rtabmap/utilite/UDirectory.h>
 #include "rtabmap/core/Memory.h"
 
 int main(int argc, char** argv)
@@ -14,7 +14,7 @@ int main(int argc, char** argv)
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
 
-	std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/LTM.db";
+	std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/rtabmap.db";
 	if(argc > 1)
 	{
 		path = argv[1];

tools/ImagesJoiner/CMakeLists.txt

@@ -4,13 +4,12 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBS} 
 )
 
@@ -20,7 +19,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 # Add binary called "imagesJoiner" that is built from the source file "main.cpp".
 # The extension is automatically found.
 ADD_EXECUTABLE(imagesJoiner ${SRC_FILES})
-TARGET_LINK_LIBRARIES(imagesJoiner ${LIBRARIES})
+TARGET_LINK_LIBRARIES(imagesJoiner rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( imagesJoiner 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesJoiner)

tools/ImagesJoiner/main.cpp

@@ -17,10 +17,10 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
-#include "utilite/UTimer.h"
+#include "rtabmap/utilite/UTimer.h"
-#include "utilite/UDirectory.h"
+#include "rtabmap/utilite/UDirectory.h"
-#include "utilite/UConversion.h"
+#include "rtabmap/utilite/UConversion.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 

tools/VocabularyComparison/CMakeLists.txt

@@ -1,19 +1,18 @@
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
-    ${UTILITE_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 )
 
 SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
 	${OpenCV_LIBRARIES}
 )
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(vocabularyComparison main.cpp)
-TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_corelib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(vocabularyComparison rtabmap_corelib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( vocabularyComparison 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-vocabularyComparison)

tools/VocabularyComparison/main.cpp

@@ -5,11 +5,11 @@
 #include <opencv2/flann/miniflann.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <opencv2/nonfree/features2d.hpp>
-#include <utilite/ULogger.h>
+#include <rtabmap/utilite/ULogger.h>
-#include <utilite/UTimer.h>
+#include <rtabmap/utilite/UTimer.h>
-#include <utilite/UConversion.h>
+#include <rtabmap/utilite/UConversion.h>
-#include <utilite/UStl.h>
+#include <rtabmap/utilite/UStl.h>
-#include <utilite/UMath.h>
+#include <rtabmap/utilite/UMath.h>
 #include <fstream>
 #include <vector>
 #include <list>

tools/viewer/CMakeLists.txt

@@ -4,6 +4,7 @@ SET(SRC_FILES
 )
 
 SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/guilib/include
     ${UTILITE_INCLUDE_DIRS}
@@ -21,7 +22,7 @@ SET(LIBRARIES
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 ADD_EXECUTABLE(databaseViewer WIN32 ${SRC_FILES})
-TARGET_LINK_LIBRARIES(databaseViewer rtabmap_corelib rtabmap_guilib ${LIBRARIES})
+TARGET_LINK_LIBRARIES(databaseViewer rtabmap_corelib rtabmap_guilib rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( databaseViewer 
   PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-databaseViewer)

tools/viewer/main.cpp

@@ -19,7 +19,7 @@
 
 #include <QtGui/QApplication>
 #include "rtabmap/gui/DatabaseViewer.h"
-#include "utilite/ULogger.h"
+#include "rtabmap/utilite/ULogger.h"
 
 int main(int argc, char * argv[])
 {

utilite/CMakeLists.txt

@@ -0,0 +1,9 @@
+SET(UTILITE_VERSION "0.3.0")
+ADD_DEFINITIONS(-DUTILITE_VERSION="${UTILITE_VERSION}")
+
+IF(UNIX)
+   FIND_PACKAGE(Pthreads REQUIRED)
+ENDIF(UNIX)
+
+ADD_SUBDIRECTORY( src )
+ADD_SUBDIRECTORY( resource_generator )

utilite/include/rtabmap/utilite/UConversion.h

@@ -0,0 +1,255 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UCONVERSION_H
+#define UCONVERSION_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include <string>
+#include <vector>
+#include <stdarg.h>
+
+/**
+ * \file UConversion.h
+ * \brief Some conversion functions
+ *
+ * This contains functions to do some convenient conversion like
+ * uNumber2str(), uBytes2Hex() or uHex2Bytes().
+*/
+
+/**
+ * Replace old characters in a string to new ones.
+ * Example :
+ * @code
+ * std::string str = "Hello";
+ * uReplaceChar(str, 'l', 'p');
+ * // The results is str = "Heppo";
+ * @endcode
+ *
+ * @param str the string
+ * @param before the character to be replaced by the new one
+ * @param after the new character replacing the old one
+ * @return the modified string
+ */
+std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, char after);
+
+
+/**
+ * Replace old characters in a string with the specified string.
+ * Example :
+ * @code
+ * std::string str = "Hello";
+ * uReplaceChar(str, 'o', "oween");
+ * // The results is str = "Helloween";
+ * @endcode
+ *
+ * @param str the string
+ * @param before the character to be replaced by the new one
+ * @param after the new string replacing the old character
+ * @return the modified string
+ */
+std::string UTILITE_EXP uReplaceChar(const std::string & str, char before, const std::string & after);
+
+
+/**
+ * Transform characters from a string to upper case.
+ * Example :
+ * @code
+ * std::string str = "hello!";
+ * str = uToUpperCase(str);
+ * //str is now equal to "HELLO!"
+ * @endcode
+ * @param str the string
+ * @return the modified string
+ */
+std::string UTILITE_EXP uToUpperCase(const std::string & str);
+
+/**
+ * Transform characters from a string to lower case.
+ * Example :
+ * @code
+ * std::string str = "HELLO!";
+ * str = uToLowerCase(str, false);
+ * //str is now equal to "hello!"
+ * @endcode
+ * @param str the string
+ * @return the modified string
+ */
+std::string UTILITE_EXP uToLowerCase(const std::string & str);
+
+/**
+ * Convert a number (unsigned int) to a string.
+ * @param number the number to convert in a string
+ * @return the string
+ */
+std::string UTILITE_EXP uNumber2Str(unsigned int number);
+/**
+ * Convert a number (int) to a string.
+ * @param number the number to convert in a string
+ * @return the string
+ */
+std::string UTILITE_EXP uNumber2Str(int number);
+/**
+ * Convert a number (float) to a string.
+ * @param number the number to convert in a string
+ * @return the string
+ */
+std::string UTILITE_EXP uNumber2Str(float number);
+/**
+ * Convert a number (double) to a string.
+ * @param number the number to convert in a string
+ * @return the string
+ */
+std::string UTILITE_EXP uNumber2Str(double number);
+
+/**
+ * Convert a bool to a string.
+ * The format used is "true" and "false".
+ * @param boolean the boolean to convert in a string
+ * @return the string
+ */
+std::string UTILITE_EXP uBool2Str(bool boolean);
+/**
+ * Convert a string to a boolean.
+ * The format used is :
+ * "false", "FALSE" or "0" give false. All others give true.
+ * @param str the string to convert in a boolean
+ * @return the boolean
+ */
+bool UTILITE_EXP uStr2Bool(const char * str);
+
+/**
+ * Convert a bytes array to an hexadecimal string.
+ * The resulting string is twice the size of the bytes array. The hexadecimal
+ * Characters are in upper case.
+ * Example :
+ * @code
+ * char bytes[] = {0x3F};
+ * std::string hex = uBytes2Hex(bytes, 1);
+ * // The string constains "3F".
+ * @endcode
+ *
+ * @param bytes the bytes array
+ * @param bytesLen the length of the bytes array
+ * @return the hexadecimal string
+ */
+std::string UTILITE_EXP uBytes2Hex(const char * bytes, unsigned int bytesLen);
+/**
+ * Convert an hexadecimal string to a bytes array.
+ * The string must be pair length. The hexadecimal
+ * Characters can be in upper or lower case.
+ * Example :
+ * @code
+ * std::string hex = "1f3B";
+ * std::vector<char> bytes = uHex2Bytes(hex);
+ * // The array contains {0x1F, 0x3B}.
+ * @endcode
+ *
+ * @param hex the hexadecimal string
+ * @return the bytes array
+ */
+std::vector<char> UTILITE_EXP uHex2Bytes(const std::string & hex);
+/**
+ * Convert an hexadecimal string to a bytes array.
+ * The string must be pair length. The hexadecimal
+ * Characters can be in upper or lower case.
+ * Example :
+ * @code
+ * std::vector<char> bytes = uHex2Bytes("1f3B", 4);
+ * // The array contains {0x1F, 0x3B}.
+ * @endcode
+ *
+ * @param hex the hexadecimal string
+ * @param bytesLen the hexadecimal string length
+ * @return the bytes array
+ */
+std::vector<char> UTILITE_EXP uHex2Bytes(const char * hex, int hexLen);
+
+/**
+ * Convert an hexadecimal string to an ascii string. A convenient way
+ * when using only strings.
+ * The hexadecimal str MUST not contains any null values 0x00 ("00").
+ * Think to use of hex2bytes() to handle 0x00 values.
+ * Characters can be in upper or lower case.
+ * Example :
+ * @code
+ * std::string str = uHex2Str("48656C6C4F21");
+ * // The string contains "Hello!".
+ * @endcode
+ *
+ * @see hex2bytes
+ * @param hex the hexadecimal string
+ * @return the ascii string
+ */
+std::string UTILITE_EXP uHex2Str(const std::string & hex);
+
+/**
+ * Convert hexadecimal (left or right part) value to an ascii character.
+ * Example :
+ * @code
+ * unsigned char F = uHex2Ascii(0xFA, false);
+ * unsigned char A = uHex2Ascii(0xFA, true);
+ * @endcode
+ * @see ascii2hex
+ * @param c the hexadecimal value
+ * @param rightPart If we want the character corresponding to the right of left part (4 bits) of the byte value.
+ * @return the ascii character (in upper case)
+ */
+unsigned char UTILITE_EXP uHex2Ascii(const unsigned char & c, bool rightPart);
+
+/**
+ * Convert an ascii character to an hexadecimal value (right 4 bits).
+ * Characters can be in upper or lower case.
+ * Example :
+ * @code
+ * unsigned char hex = uAscii2Hex('F');
+ * // The results is hex = 0x0F;
+ * @endcode
+ * @see hex2ascii
+ * @param c the ascii character
+ * @return the hexadecimal value
+ */
+unsigned char UTILITE_EXP uAscii2Hex(const unsigned char & c);
+
+/**
+ * Format a string like printf, and return it as a std::string
+ */
+std::string UTILITE_EXP uFormatv (const char *fmt, va_list ap);
+
+/**
+ * Format a string like printf, and return it as a std::string
+ */
+std::string UTILITE_EXP uFormat (const char *fmt, ...);
+
+#ifdef WIN32
+/**
+ * Convert multi-byte string to unicode (wide-char) string.
+ * Note that returned whar_t * must be deleted : delete [] wText;
+ */
+wchar_t * UTILITE_EXP createWCharFromChar(const char * text);
+
+/**
+ * Convert unicode (wide-char) string to multi-byte string.
+ * Note that returned char * must be deleted : delete [] text;
+ */
+char * UTILITE_EXP createCharFromWChar(const wchar_t * wText);
+#endif
+
+#endif /* UCONVERSION_H */

utilite/include/rtabmap/utilite/UDestroyer.h

@@ -0,0 +1,77 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UDESTROYER_H
+#define UDESTROYER_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+/**
+ * This class is used to delete a dynamically created
+ * objects. It was mainly designed to remove dynamically created Singleton.
+ * Created on the stack of a Singleton, when the
+ * application is finished, his destructor make sure that the 
+ * Singleton is deleted.
+ *
+ */
+template <class T>
+class UDestroyer
+{
+public:
+	/**
+	 * The constructor. Set the doomed object (take ownership of the object). The object is deleted
+	 * when this object is deleted.
+	 */
+    UDestroyer(T* doomed = 0)  : doomed_(doomed) {}
+    
+    ~UDestroyer()
+    {
+        if(doomed_)
+        {
+            delete doomed_;
+            doomed_ = 0;
+        }
+    }
+
+    /**
+	 * Set the doomed object. If a doomed object is already set, the function returns false.
+	 * @param doomed the doomed object
+	 * @return false if an object is already set and the new object is not null, otherwise true
+	 */
+    bool setDoomed(T* doomed)
+	{
+    	if(doomed_ && doomed)
+    	{
+    		return false;
+    	}
+		doomed_ = doomed;
+		return true;
+	}
+
+private:
+    // Prevent users from making copies of a 
+    // Destroyer to avoid double deletion:
+    UDestroyer(const UDestroyer<T>&);
+    void operator=(const UDestroyer<T>&);
+
+private:
+    T* doomed_;
+};
+
+#endif // UDESTROYER_H

utilite/include/rtabmap/utilite/UDirectory.h

@@ -0,0 +1,137 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UDIRECTORY_H
+#define UDIRECTORY_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include <string>
+#include <vector>
+#include <list>
+
+/**
+ * Class UDirectory.
+ *
+ * This class can be used to get file names in a directory.
+ */
+class UTILITE_EXP UDirectory
+{
+public:
+	/**
+	 * Check if a directory exists.
+	 * @param dirPath the directory path
+	 * @return true if the directory exists
+	 */
+	static bool exists(const std::string & dirPath);
+
+	/**
+	 * Get the directory path of a file path.
+	 * @param filePath the file path
+	 * @return the directory path of the file
+	 */
+	static std::string getDir(const std::string & filePath);
+
+	/**
+	 * Get the current directory.
+	 * @param trailingSeparator If true, a '/' is added to the path.
+	 * @return the current directory
+	 */
+	static std::string currentDir(bool trailingSeparator = false);
+
+	/**
+	 * Make a directory.
+	 * @param dirPath the directory path
+	 * @return true on success, false otherwise.
+	 */
+	static bool makeDir(const std::string & dirPath);
+
+	/**
+	 * Remove a directory.
+	 * @param dirPath the directory path
+	 * @return true on success, false otherwise.
+	 */
+	static bool removeDir(const std::string & dirPath);
+
+	/**
+	 * Return the "home" directory.
+	 * @return the directory path.
+	 */
+	static std::string homeDir();
+
+	/**
+	 * Return \ (Win32) or / (Unix) depending of the platform.
+	 */
+	static std::string separator();
+
+public:
+	/**
+	 * Create a UDirectory object with path initialized to an existing "path" and with filter "extensions".
+	 * @param path the path to an existing directory
+	 * @param extensions filter to get only file names with the extensions specified, format is a
+	 * list of extensions separated by a space: "jpg bmp" get only file names finishing by jpg or bmp.
+	 */
+	UDirectory(const std::string & path = "", const std::string & extensions = "");
+	UDirectory(const UDirectory & dir);
+	UDirectory & operator=(const UDirectory & dir);
+	~UDirectory();
+
+	/**
+	 * Set path of the directory.
+	 * @param path the new directory path.
+	 */
+	void setPath(const std::string & path, const std::string & extensions = "");
+
+	/**
+	 * Update indexed file names (if the directory changed).
+	 */
+	void update();
+
+	/**
+	 * Check is the directory exists.
+	 * @return if directory exists.
+	 */
+	bool isValid();
+
+	/**
+	 * Get the next file name.
+	 * @return the next file name
+	 */
+	std::string getNextFileName();
+
+	/**
+	 * Get all file names.
+	 * @see UDirectory()
+	 * @return all the file names in directory matching the set extensions.
+	 */
+	const std::list<std::string> & getFileNames() const {return fileNames_;}
+
+	/**
+	 * Return the pointer of file names to beginning.
+	 */
+	void rewind();
+
+private:
+	std::string path_;
+	std::vector<std::string> extensions_;
+	std::list<std::string> fileNames_;
+	std::list<std::string>::iterator iFileName_;
+};
+
+#endif /* UDIRECTORY_H */

utilite/include/rtabmap/utilite/UEvent.h

@@ -0,0 +1,87 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UEVENT_H
+#define UEVENT_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include <string>
+
+class UEventsHandler;
+
+/**
+ * This is the base class for all events used 
+ * with the UEventsManager. Inherited classes
+ * must redefined the virtual method getClassName() 
+ * to return their class name.
+ *
+ * Example:
+ * @code
+ *  class MyEvent : public UEvent
+ *  {
+ *  public:
+ *     MyEvent() {}
+ *     virtual ~MyEvent() {}
+ *     std::string getClassName() const {return "MyEvent";}
+ *  };
+ *
+ *  int main(int argc, char * argv[])
+ *  {
+ *  	...
+ *  	UEventsManager::post(new MyEvent()); // UEventsManager take ownership of the event (deleted by UEventsManager).
+ *  	...
+ *  }
+ * @endcode
+ *
+ * @see UEventsManager
+ * @see UEventsHandler
+ * @see getClassName()
+ */
+class UTILITE_EXP UEvent{
+public:
+    virtual ~UEvent() {}
+
+    /**
+     * This method is used to get the class name 
+     * of the event. For example, if a class MouseEvent
+     * inherits from UEvent, it must return the
+     * "MouseEvent" string.
+     * @return the class name
+     */
+    virtual std::string getClassName() const = 0; // TODO : macro?
+
+    /**
+     * Get event's code.
+     * @return the code
+     */
+    int getCode() const {return code_;}
+
+protected:
+    /**
+	 * @param code the event code.
+	 * TODO : Remove the code, not required for most of all implemented events
+	 */
+	UEvent(int code = 0) : code_(code) {}
+
+private:
+    int code_; /**< The event's code. */
+};
+
+#endif // UEVENT_H

utilite/include/rtabmap/utilite/UEventsHandler.h

@@ -0,0 +1,184 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UEVENTSHANDLER_H
+#define UEVENTSHANDLER_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+class UEvent;
+
+/**
+ * The class UEventsHandler is an abstract class for
+ * handling events.
+ *
+ * Inherited classes must implement handleEvent() function, which
+ * is called by the UEventsManager when an event is dispatched. Once the handler is
+ * created, it must be added to events manager with UEventsManager::addHandler() function.
+ * Note that it is not safe to automatically add the handler to UEventsManager in the handler's constructor.
+ *
+ * Note for multi-threading: the handleEvent() method is called
+ * inside the UEventsManager thread. If the inherited class also inherits
+ * from UThreadNode, handleEvent() is done as well outside the thread's main loop, so
+ * be careful to protect private data of the thread used in its main loop.
+ *
+ * Example for a useful combination of an UEventsHandler and a UThreadNode, with safe data
+ * modification while not blocking the handleEvent() call on a mutex:
+ * @code
+ * #include "utilite/UThreadNode.h"
+ * #include "utilite/UEventsHandler.h"
+ * #include "utilite/UEventsManager.h"
+ * #include "utilite/UEvent.h"
+ *
+ * // Implement a simple event
+ * class ResetEvent : public UEvent {
+ * public:
+ *    ResetEvent() {}
+ *    virtual ~ResetEvent() {}
+ *    virtual std::string getClassName() const {return "ResetEvent";} // Must be implemented
+ * };
+ *
+ * // There is the thread counting indefinitely, the count can be reseted by sending a ResetEvent.
+ * class CounterThread : public UThreadNode, public UEventsHandler {
+ * public:
+ *    CounterThread() : state_(0), count_(0) {}
+ *    virtual ~CounterThread() {this->join(true);}
+ *
+ * protected:
+ *    virtual void mainLoop() {
+ *       if(state_ == 1) {
+ *          state_ = 0;
+ *          // Do a long initialization, reset memory or other special long works... here
+ *          // we reset the count. This could be done in the handleEvent() but
+ *          // with many objects, it is more safe to do it here (in the thread's loop). A safe
+ *          // way could be also to use a UMutex to protect this initialization in
+ *          // the handleEvent(), but it is not recommended to do long works in handleEvent()
+ *          // because this will add latency in the UEventsManager dispatching events loop.
+ *          count_ = 0; // Reset the count
+ *          printf("Reset!\n");
+ *       }
+ *
+ *       // Do some works...
+ *       printf("count=%d\n", count_++);
+ *       uSleep(100); // wait 100 ms
+ *    }
+ *    virtual void handleEvent(UEvent * event) {
+ *       if(event->getClassName().compare("ResetEvent") == 0) {
+ *          state_ = 1;
+ *       }
+ *    }
+ * private:
+ *    int state_;
+ *    int count_;
+ * };
+ *
+ * int main(int argc, char * argv[])
+ * {
+ *    CounterThread counter;
+ *    counter.start();
+ *    UEventsManager::addHandler(&counter);
+ *
+ *    uSleep(500); // wait 500 ms before sending a reset event
+ *    UEventsManager::post(new ResetEvent());
+ *    uSleep(500); // wait 500 ms before termination
+ *
+ *    UEventsManager::removeHandler(&counter);
+ *    counter.join(true); // Kill and wait to finish
+ *    return 0;
+ * }
+ * @endcode
+ *
+ * The output is:
+ * @code
+ * count=0
+ * count=1
+ * count=2
+ * count=3
+ * count=4
+ * Reset!
+ * count=0
+ * count=1
+ * count=2
+ * count=3
+ * count=4
+ * @endcode
+ *
+ * @see UEventsManager
+ * @see UEvent
+ * @see UThreadNode
+ *
+ */
+class UTILITE_EXP UEventsHandler{
+public:
+    
+
+protected:
+    /**
+     * Only the UEventsManager has access
+     * to the handleEvent() method.
+     */
+    friend class UEventsManager;
+
+    /**
+     * Method called by the UEventsManager
+     * to handle an event. Important : this method 
+     * must do a minimum of work because the faster 
+     * the dispatching loop is done; the faster the 
+     * events are received. If a handling function 
+     * takes too much time, the events list can grow 
+     * faster than it is emptied. The event can be
+     * modified but must not be deleted.
+     *
+     */
+    virtual void handleEvent(UEvent * event) = 0;
+
+protected:
+    /**
+     * UEventsHandler constructor.
+     *
+     * Note : You can call EventsManager::addHandler(this) at
+     * the end of the constructor of the inherited class where the virtual
+     * method handleEvent(...) is defined. If so, the UEventsHandler doesn't
+     * need to be manually added to the EventsManager where the handler
+     * is instantiated. We decided to not include UEventsManager::addHandler(this)
+     * in this abstract class constructor because an event can be handled (calling
+     * the pure virtual method) while the concrete class is constructed.
+     */
+    UEventsHandler() {}
+
+    /**
+     * UEventsHandler destructor.
+     *
+     * By default, it removes the handler reference from the UEventsManager. To be thread-safe,
+     * the inherited class must remove itself from the UEventsManager before it is deleted because
+     * an event can be handled (calling the pure virtual method handleEvent()) after the concrete class
+     * is deleted.
+     */
+    virtual ~UEventsHandler();
+
+    /**
+     * For convenience to post an event. This is the same than calling UEventsManager::post().
+     */
+    void post(UEvent * event, bool async = true);
+
+private:
+    
+};
+
+#endif // UEVENTSHANDLER_H

utilite/include/rtabmap/utilite/UEventsManager.h

@@ -0,0 +1,207 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UEVENTSMANAGER_H
+#define UEVENTSMANAGER_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UThreadNode.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UDestroyer.h"
+
+#include <list>
+
+// TODO Not implemented... for multithreading event handling
+class UEventDispatcher : public UThread
+{
+public:
+	virtual ~UEventDispatcher();
+protected:
+	friend class UEventsManager;
+	UEventDispatcher();
+
+	virtual void mainLoop();
+
+private:
+	virtual void killCleanup();
+
+private:
+	UEvent * _event;
+	std::vector<UEventsHandler*> _handlers;
+};
+
+/**
+ * This class is used to post events between threads 
+ * in the application. It is Thread-Safe and the events are sent 
+ * to receivers in the same order they are posted (FIFO). It works 
+ * like the design pattern Mediator. It is also a Singleton, so 
+ * it can be used anywhere in the application. 
+ *
+ * To send an event, use UEventsManager::post().
+ * Events are automatically deleted after they are posted.
+ *
+ * The EventsManager have a list of handlers to which 
+ * it sends posted events. To add an handler, use 
+ * UEventsManager::addHandler(). To remove, use
+ * UEventsManager::removeHandler().
+ *
+ * @code
+ *  // Anywhere in the code:
+ *  UEventsManager::post(new MyEvent()); // where MyEvent is an implemented UEvent
+ * @endcode
+ *
+ * @see UEvent
+ * @see UEventsHandler
+ * @see post()
+ * @see addHandler()
+ * @see removeHandler()
+ */
+class UTILITE_EXP UEventsManager : public UThread{
+
+public:
+
+    /**
+     * This method is used to add an events 
+     * handler to the list of handlers.
+     *
+     * @param handler the handler to be added.
+     */
+    static void addHandler(UEventsHandler* handler);
+
+    /**
+     * This method is used to remove an events 
+     * handler from the list of handlers.
+     *
+     * @param handler the handler to be removed.
+     */
+    static void removeHandler(UEventsHandler* handler);
+
+    /**
+     * This method is used to post an event to
+     * handlers.
+     *
+     * Event can be posted asynchronously or not. In the first case,
+     * the event is dispatched by the UEventsManager's thread. In the
+     * second case, the event is handled immediately by event's
+     * receivers, thus in the sender thread.
+     *
+     * @param event the event to be posted.
+     * @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
+     */
+    static void post(UEvent * event, bool async = true);
+    
+protected:
+
+    /*
+     * This method is used to have a reference on the 
+     * EventsManager. When no EventsManager exists, one is 
+     * created. There is only one instance in the application.
+     * See the Singleton pattern further explanation.
+     *
+     * @return the reference on the EventsManager
+     */
+    static UEventsManager* getInstance();
+
+    /*
+     * Called only once in getInstance(). It can't be instantiated
+     * by the user.
+     *
+     */
+    UEventsManager();
+
+    /*
+     * Only called by a Destroyer.
+     */
+    virtual ~UEventsManager();
+
+    /*
+     * A Destroyer is used to remove a dynamically created
+     * Singleton. It is friend here to have access to the 
+     * destructor.
+     *
+     */
+    friend class UDestroyer<UEventsManager>;
+
+    /**
+	 * The UEventsManager's main loop.
+	 */
+    virtual void mainLoop();
+
+private:
+
+    /**
+	 * Reimplemented to wake up UEventsManager on termination.
+	 */
+    virtual void mainLoopKill();
+
+    /*
+     * This method dispatches asynchronized events to all handlers.
+     * FIFO (first in first out) dispatching is used.
+     */
+    virtual void dispatchEvents();
+
+    /*
+	 * This method dispatches an event to all handlers.
+	 */
+    virtual void dispatchEvent(UEvent * event);
+
+    /*
+     * This method is used to add an events 
+     * handler to the list of handlers.
+     *
+     * @param handler the handler to be added.
+     */
+    void _addHandler(UEventsHandler* handler);
+
+    /*
+     * This method is used to remove an events 
+     * handler from the list of handlers.
+     *
+     * @param handler the handler to be removed.
+     */
+    void _removeHandler(UEventsHandler* handler);
+
+    /*
+     * This method is used to post an event to
+     * handlers.
+     *
+     * Event can be posted asynchronously or not. In the first case,
+     * the event is dispatched by the UEventsManager's thread. In the
+     * second case, the event is handled immediately by event's
+     * receivers, thus in the sender thread.
+     *
+     * @param event the event to be posted.
+     * @param async if true, the event is dispatched by the UEventsManager thread, otherwise it's in the caller thread (synchronous).
+     */
+    void _postEvent(UEvent * event, bool async = true);
+    
+private:
+    
+    static UEventsManager* instance_;            /* The EventsManager instance pointer. */
+    static UDestroyer<UEventsManager> destroyer_; /* The EventsManager's destroyer. */
+    std::list<UEvent*> events_;                /* The events list. */
+    std::list<UEventsHandler*> handlers_;      /* The handlers list. */
+    UMutex eventsMutex_;                         /* The mutex of the events list, */
+    UMutex handlersMutex_;                       /* The mutex of the handlers list. */
+    USemaphore postEventSem_;                    /* Semaphore used to signal when an events is posted. */
+};
+
+#endif // UEVENTSMANAGER_H

utilite/include/rtabmap/utilite/UFile.h

@@ -0,0 +1,135 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FILE_H
+#define FILE_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UDirectory.h"
+#include <string>
+
+/**
+ * Class UFile.
+ *
+ * This class can be used to modify/erase files on hard drive.
+ */
+class UTILITE_EXP UFile
+{
+public:
+	/**
+	 * Check if a file exists.
+	 * @param filePath the file path
+	 * @return true if the file exists, otherwise false.
+	 */
+	static bool exists(const std::string &filePath);
+
+	/**
+	 * Get the file length.
+	 * @param filePath the file path
+	 * @return long the length of the file in bytes. Return -1 if the file doesn't exist.
+	 */
+	static long length(const std::string &filePath);
+
+	/**
+	 * Erase a file.
+	 * @param filePath the file path
+	 * @return 0 if success.
+	 */
+	static int erase(const std::string &filePath);
+
+	/**
+	 * Rename a file.
+	 * @param oldFilePath the old file path
+	 * @param newFilePath the new file path
+	 * @return 0 if success.
+	 */
+	static int rename(const std::string &oldFilePath,
+						  const std::string &newFilePath);
+
+	/**
+	 * Get the file name from a file path (with extension).
+	 * @param filePath the file path
+	 * @return the file name.
+	 */
+	static std::string getName(const std::string & filePath);
+
+	static std::string getExtension(const std::string &filePath);
+
+public:
+	/**
+	 * Create a UFile object with path initialized to an existing file .
+	 * @param path the path to an existing file
+	 */
+	UFile(const std::string & path) : path_(path) {}
+	~UFile() {}
+
+	/**
+	 * Check if the file exists. Same as exists().
+	 * @return true if the path exits
+	 */
+	bool isValid() {return exists(path_);}
+
+	/**
+	 * Check if the file exists.
+	 * @return true if the path exits
+	 */
+	bool exists() {return exists(path_);}
+
+	/**
+	 * Get the length of the file.
+	 * @return long the length of the file in bytes. Return -1 if the file doesn't exist.
+	 */
+	long length() {return length(path_);}
+
+	/**
+	 * Rename the file name. The path stays the same.
+	 * @param the new name
+	 */
+	int rename(const std::string &newName)
+	{
+		std::string ext = this->getExtension();
+		std::string newPath = UDirectory::getDir(path_) + std::string("/") + newName;
+		if(ext.size())
+		{
+			newPath += std::string(".") + getExtension(path_);
+		}
+		int result = rename(path_, newPath);
+		if(result == 0)
+		{
+			path_ = newPath;
+		}
+		return result;
+	}
+	/**
+	 * Get the file name without the path.
+	 * @return the file name
+	 */
+	std::string getName() {return getName(path_);}
+	/**
+	 * Get the file extension.
+	 * @return the file extension
+	 */
+	std::string getExtension() {return getExtension(path_);}
+
+private:
+	std::string path_;
+};
+
+#endif

utilite/include/rtabmap/utilite/ULogger.h

@@ -0,0 +1,562 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef ULOGGER_H
+#define ULOGGER_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/UDestroyer.h"
+#include "rtabmap/utilite/UEvent.h"
+
+#include <stdio.h>
+#include <time.h>
+#include <string>
+#include <vector>
+
+#include <stdarg.h>
+
+/**
+ * \file ULogger.h
+ * \brief ULogger class and convenient macros
+ *
+ * This contains macros useful for logging a message anywhere in the
+ * application. Once the ULogger is set, use these macros like a printf to
+ * print debug messages.
+*/
+
+/*
+ * Convenient macros for logging...
+ */
+#define ULOGGER_LOG(level, ...) ULogger::write(level, __FILE__, __LINE__, __FUNCTION__, __VA_ARGS__)
+
+#define ULOGGER_DEBUG(...)   ULOGGER_LOG(ULogger::kDebug,   __VA_ARGS__)
+#define ULOGGER_INFO(...)    ULOGGER_LOG(ULogger::kInfo,    __VA_ARGS__)
+#define ULOGGER_WARN(...) 	 ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__)
+#define ULOGGER_ERROR(...)   ULOGGER_LOG(ULogger::kError,   __VA_ARGS__)
+#define ULOGGER_FATAL(...)   ULOGGER_LOG(ULogger::kFatal,   __VA_ARGS__)
+
+#define UDEBUG(...)   ULOGGER_DEBUG(__VA_ARGS__)
+#define UINFO(...)    ULOGGER_INFO(__VA_ARGS__)
+#define UWARN(...) 	  ULOGGER_WARN(__VA_ARGS__)
+#define UERROR(...)   ULOGGER_ERROR(__VA_ARGS__)
+#define UFATAL(...)   ULOGGER_FATAL(__VA_ARGS__)
+
+#define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition)
+#define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str)
+
+/**
+ * \def UDEBUG(...)
+ * Print a debug level message in the logger. Format is the same as a printf:
+ * @code
+ * UDEBUG("This is a debug message with the number %d", 42);
+ * @endcode
+ */
+/**
+ * \def UINFO(...)
+ * Print a information level message in the logger. Format is the same as a printf:
+ * @code
+ * UINFO("This is a information message with the number %d", 42);
+ * @endcode
+ */
+/**
+ * \def UWARN(...)
+ * Print a warning level message in the logger. Format is the same as a printf:
+ * @code
+ * UWARN("This is a warning message with the number %d", 42);
+ * @endcode
+ */
+/**
+ * \def UERROR(...)
+ * Print an error level message in the logger. Format is the same as a printf:
+ * @code
+ * UERROR("This is an error message with the number %d", 42);
+ * @endcode
+ */
+/**
+ * \def UFATAL(...)
+ * Print a fatal error level message in the logger. The application will exit on
+ * fatal error. Format is the same as a printf:
+ * @code
+ * UFATAL("This is a fatal error message with the number %d", 42);
+ * @endcode
+ */
+/**
+ * \def UASSERT(condition, ...)
+ * Print a fatal error level message in the logger if condition is not met. The application will exit on
+ * fatal error. Format is the same as a printf:
+ * @code
+ * UASSERT(a!=42, "This is a fatal error message with the number %d", 42);
+ * @endcode
+ */
+
+
+
+/**
+ * This class is used by the ULogger to send logged messages like events. Messages with level
+ * over the event level set in ULogger::setEventLevel() are sent like ULogEvent with the message and its level.
+ * The default event level of ULogger is kFatal (see ULogger::Level).
+ */
+class ULogEvent : public UEvent
+{
+public:
+	/**
+	 * ULogEvent constructor. Note that to retrieve the message level, use UEvent::getCode().
+	 * @param msg the message already formatted to a full string.
+	 * @param level the severity of the message, @see ULogger::Level.
+	 */
+	ULogEvent(const std::string & msg, int level) :
+		UEvent(level),
+		msg_(msg)
+	{}
+	virtual ~ULogEvent() {}
+	/**
+	 * Get the message from the event.
+	 */
+	const std::string & getMsg() const {return msg_;}
+	/**
+	 * @return string "ULogEvent"
+	 */
+	virtual std::string getClassName() const {return "ULogEvent";}
+private:
+	std::string msg_;
+};
+
+/**
+ * This class is used to log messages with time on a console, in a file 
+ * and/or with an event. At the start of the application, call
+ * ULogger::setType() with the type of the logger you want (see ULogger::Type, the type of the output
+ * can be changed at the run-time.). To use it,
+ * simply call the convenient macros UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL() depending of
+ * the severity of the message. You can disable some messages by setting the logger
+ * level ULogger::setLevel() to severity you want, defined by ULogger::Level. A fatal message
+ * will make the application to exit, printing the message on console (whatever the logger type) and
+ * posting a ULogEvent (synchronously... see UEventsManager::post()) before exiting.
+ *
+ * The display of the logged messages can be modified:
+ * - If you don't want the level label, set ULogger::setPrintLevel() to false.
+ * - If you don't want the time label, set ULogger::setPrintTime() to false.
+ * - If you don't want the end of line added, set ULogger::setPrintEndline() to false.
+ * - If you don't the full path of the message, set ULogger::setPrintWhereFullPath() to false.
+ * - If you don't the path of the message, set ULogger::setPrintWhere() to false.
+ *
+ * When using a file logger (kTypeLogger), it can be useful in some
+ * application to buffer messages before writing them to hard drive (avoiding
+ * hard drive latencies). You can set ULogger::setBuffered() to true to do that. When the
+ * buffered messages will be written to file on appllciation exit (ULogger destructor) or when
+ * ULogger::flush() is called.
+ *
+ * If you want the application to exit on a lower severity level than kFatal,
+ * you can set ULogger::setExitLevel() to any ULogger::Type you want.
+ *
+ * Example:
+ * @code
+ * #include <utilite/ULogger.h>
+ * int main(int argc, char * argv[])
+ * {
+ *    // Set the logger type. The choices are kTypeConsole,
+ *    // kTypeFile or kTypeNoLog (nothing is logged).
+ *    ULogger::setType(ULogger::kTypeConsole);
+ *
+ *    // Set the logger severity level (kDebug, kInfo, kWarning, kError, kFatal).
+ *    // All log entries under the severity level are not logged. Here,
+ *    // only debug messages are not logged.
+ *    ULogger::setLevel(ULogger::kInfo);
+ *
+ *    // Use a predefined Macro to easy logging. It can be
+ *    // called anywhere in the application as the logger is
+ *    // a Singleton.
+ *    UDEBUG("This message won't be logged because the "
+ *           "severity level of the logger is set to kInfo.");
+ *
+ *    UINFO("This message is logged.");
+ *
+ *    UWARN("A warning message...");
+ *
+ *    UERROR("An error message with code %d.", 42);
+ *
+ *    return 0;
+ * }
+ * @endcode
+ * Output:
+ * @code
+ * [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() This message is logged.
+ * [ WARN] (2010-09-25 18:08:20) main.cpp:20::main() A warning message...
+ * [ERROR] (2010-09-25 18:08:20) main.cpp:22::main() An error message with code 42.
+ * @endcode
+ *
+ * Another useful form of the ULogger is to use it with the UTimer class. Here an example:
+ * @code
+ * #include <utilite/ULogger.h>
+ * #include <utilite/UTimer.h>
+ * ...
+ * UTimer timer; // automatically starts
+ * // do some works for part A
+ * UINFO("Time for part A = %f s", timer.ticks());
+ * // do some works for part B
+ * UINFO("Time for part B = %f s", timer.ticks());
+ * // do some works for part C
+ * UINFO("Time for part C = %f s", timer.ticks());
+ * ...
+ * @endcode
+ *
+ * @see setType()
+ * @see setLevel()
+ * @see UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL()
+ *
+ */
+class UTILITE_EXP ULogger
+{
+
+public:
+    /**
+     * The default log file name.
+     */
+    static const std::string kDefaultLogFileName;
+
+    /**
+     * Loggers available:
+     * @code
+     * kTypeNoLog, kTypeConsole, kTypeFile
+     * @endcode
+     */
+    enum Type{kTypeNoLog, kTypeConsole, kTypeFile};
+
+    /**
+     * Logger levels, from lowest severity to highest:
+     * @code
+     * kDebug, kInfo, kWarning, kError, kFatal
+     * @endcode
+     */
+    enum Level{kDebug, kInfo, kWarning, kError, kFatal};
+
+    /**
+     * Set the type of the logger. When using kTypeFile, the parameter "fileName" would be
+     * changed (default is "./ULog.txt"), and optionally "append" if we want the
+     * logger to append messages to file or to overwrite the file.
+     * @param type the ULogger::Type of the logger.
+     * @param fileName file name used with a file logger type.
+     * @param append if true, the file isn't overwritten, otherwise it is.
+     *
+     * TODO : Can it be useful to have 2 or more types at the same time ? Print
+     *         in console and file at the same time.
+     */
+    static void setType(Type type, const std::string &fileName = kDefaultLogFileName, bool append = true);
+    static Type type() {return type_;}
+
+    // Setters
+    /**
+     * Print time: default true.
+     * @param printTime true to print time, otherwise set to false.
+     */
+    static void setPrintTime(bool printTime) {printTime_ = printTime;}
+    static bool isPrintTime() {return printTime_;}
+
+    /**
+     * Print level: default true.
+	 * @param printLevel true to print level, otherwise set to false.
+	 */
+    static void setPrintLevel(bool printLevel) {printLevel_ = printLevel;}
+    static bool isPrintLevel() {return printLevel_;}
+
+    /**
+     * Print end of line: default true.
+	 * @param printLevel true to print end of line, otherwise set to false.
+	 */
+    static void setPrintEndline(bool printEndline) {printEndline_ = printEndline;}
+    static bool isPrintEndLine() {return printEndline_;}
+
+    /**
+	 * Print text with color: default true.
+	 * Dark green for Debug, white for Info, yellow for Warning, red for Error and Fatal.
+	 * @param printColored true to print text with color, otherwise set to false.
+	 */
+	static void setPrintColored(bool printColored) {printColored_ = printColored;}
+	static bool isPrintColored() {return printColored_;}
+
+    /**
+     * Print where is this message in source code: default true.
+	 * @param printWhere true to print where, otherwise set to false.
+	 */
+    static void setPrintWhere(bool printWhere) {printWhere_ = printWhere;}
+    static bool isPrintWhere() {return printWhere_;}
+
+    /**
+     * Print the full path: default true. ULogger::setPrintWhere() must be true to have path printed.
+	 * @param printWhereFullPath true to print the full path, otherwise set to false.
+	 */
+    static void setPrintWhereFullPath(bool printWhereFullPath) {printWhereFullPath_ = printWhereFullPath;}
+    static bool isPrintWhereFullPath() {return printWhereFullPath_;}
+
+    /**
+     * Set is the logger buffers messages, default false. When true, the messages are
+     * buffered until the application is closed or ULogger::flush() is called.
+     * @see ULogger::flush()
+	 * @param buffered true to buffer messages, otherwise set to false.
+	 */
+    static void setBuffered(bool buffered);
+    static bool isBuffered() {return buffered_;}
+
+    /**
+     * Set logger level: default kInfo. All messages over the severity set
+     * are printed, other are ignored. The severity is from the lowest to
+     * highest:
+     * - kDebug
+     * - kInfo
+     * - kWarning
+     * - kError
+     * - kFatal
+     * @param level the minimum level of the messages printed.
+     */
+    static void setLevel(ULogger::Level level) {level_ = level;}
+    static ULogger::Level level() {return level_;}
+
+    /**
+	 * Make application to exit when a log with level is written (useful for debugging). The message is printed to
+	 * console (whatever the logger type) and an ULogEvent is sent (synchronously... see UEventsManager::post()) before exiting.
+	 *
+	 * Note : A kFatal level will always exit whatever the level specified here.
+	 */
+	static void setExitLevel(ULogger::Level exitLevel) {exitLevel_ = exitLevel;}
+	static ULogger::Level exitLevel() {return exitLevel_;}
+
+	/**
+	 * An ULogEvent is sent on each message logged at the specified level.
+	 * Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()).
+	 * @see ULogEvent
+	 * @see setExitLevel()
+	 */
+	static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;}
+	static ULogger::Level eventLevel() {return eventLevel_;}
+
+    /**
+     * Reset to default parameters.
+     */
+    static void reset();
+
+    /**
+	 * Flush buffered messages.
+	 * @see setBuffered()
+	 */
+	static void flush();
+
+    /**
+     * Write a message directly to logger without level handling.
+     * @param msg the message to write.
+     * @param ... the variable arguments
+     * @deprecated use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL()
+     */
+    static void write(const char* msg, ...);
+
+    /*
+     * Write a message to logger: use UDEBUG(), UINFO(), UWARN(), UERROR() or UFATAL() instead.
+     * @param level the log level of this message
+     * @param file the file path
+     * @param line the line in the file
+     * @param function the function name in which the message is logged
+     * @param msg the message to write
+     * @param ... the variable arguments
+     */
+    static void write(ULogger::Level level,
+    		const char * file,
+    		int line,
+    		const char *function,
+    		const char* msg,
+    		...);
+
+    /**
+     * Get the time in the format "2008-7-13 12:23:44".
+     * @param timeStr string were the time will be copied.
+     * @return the number of characters written, or 0 if an error occurred.
+     */
+    static int getTime(std::string &timeStr);
+
+protected:
+    /*
+     * This method is used to have a reference on the 
+     * Logger. When no Logger exists, one is 
+     * created. There is only one instance in the application.
+     * Must be protected by loggerMutex_.
+     * See the Singleton pattern for further explanation.
+     *
+     * @return the reference on the Logger
+     */
+    static ULogger* getInstance();
+
+    /*
+     * Called only once in getInstance(). It can't be instanciated 
+     * by the user.
+     *
+     * @see getInstance()
+     */
+    ULogger() {}
+
+    /*
+     * Only called by a Destroyer.
+     * @see Destroyer
+     */
+    virtual ~ULogger();
+
+    /*
+	 * Flush buffered messages
+	 */
+	void _flush();
+
+    /*
+     * A Destroyer is used to remove a dynamicaly created 
+     * Singleton. It is friend here to have access to the 
+     * destructor.
+     *
+     * @see Destroyer
+     */
+    friend class UDestroyer<ULogger>;
+    
+    /*
+     * The log file name.
+     */
+    static std::string logFileName_;
+
+    /*
+     * Default true, it doesn't overwrite the file.
+     */
+    static bool append_;
+    
+private:
+    /*
+     * Create an instance according to type. See the Abstract factory 
+     * pattern for further explanation.
+     * @see type_
+     * @return the reference on the new logger
+     */
+    static ULogger* createInstance();
+
+    /*
+     * Write a message on the output with the format :
+     * "A message". Inherited class
+     * must override this method to output the message. It 
+     * does nothing by default.
+     * @param msg the message to write.
+     * @param arg the variable arguments
+     */
+    virtual void _write(const char* msg, va_list arg) {} // Do nothing by default
+
+private:
+    /*
+     * The Logger instance pointer.
+     */
+    static ULogger* instance_;
+
+    /*
+     * The Logger's destroyer
+     */
+    static UDestroyer<ULogger> destroyer_;
+
+    /*
+     * If the logger prints the time for each message. 
+     * Default is true.
+     */
+    static bool printTime_;
+
+    /*
+     * If the logger prints the level for each message. 
+     * Default is true.
+     */
+    static bool printLevel_;
+
+    /*
+     * If the logger prints the end line for each message. 
+     * Default is true.
+     */
+    static bool printEndline_;
+
+    /*
+	 * If the logger prints text with color.
+	 * Default is true.
+	 */
+    static bool printColored_;
+
+    /*
+	 * If the logger prints where the message is logged (fileName::function():line).
+	 * Default is true.
+	 */
+    static bool printWhere_;
+
+    /*
+	 * If the logger prints the full path of the source file
+	 * where the message is written. Only works when
+	 * "printWhere_" is true.
+	 * Default is false.
+	 */
+    static bool printWhereFullPath_;
+
+    /*
+	 * If the logger limit the size of the "where" path to
+	 * characters. If the path is over 8 characters, a "~"
+	 * is added. Only works when "printWhereFullPath_" is false.
+	 * Default is false.
+	 */
+    static bool limitWhereLength_;
+
+    /*
+     * The type of the logger.
+     */
+    static Type type_;
+
+    /*
+	 * The severity of the log.
+	 */
+    static Level level_;
+
+    /*
+     * The severity at which the application exits.
+     * Note : A FATAL level will always exit whatever the level specified here.
+     */
+    static Level exitLevel_;
+
+    /*
+	 * The severity at which the message is also sent in a ULogEvent.
+	 */
+	static Level eventLevel_;
+
+    static const char * levelName_[5];
+
+    /*
+     * Mutex used when accessing public functions.
+     */
+    static UMutex loggerMutex_;
+
+    /*
+	 * If the logger prints messages only when ULogger::flush() is called.
+	 * Default is false.
+	 */
+	static bool buffered_;
+
+	static std::string bufferedMsgs_;
+
+	/*
+	 * State attribute. This state happens when an exit level
+	 * message is received.
+	 * Messages received during this state are not logged.
+	 * @see exitLevel_
+	 */
+	static bool exitingState_;
+};
+
+#endif // ULOGGER_H

utilite/include/rtabmap/utilite/UMath.h

@@ -0,0 +1,829 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UMATH_H
+#define UMATH_H
+
+/** \file UMath.h
+    \brief Basic mathematics functions
+*/
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include <cmath>
+#include <list>
+#include <vector>
+
+/**
+ * Get the maximum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @param index the index of the maximum value in the vector.
+ * @return the maximum value of the array
+ */
+template<class T>
+inline T uMax(const T * v, unsigned int size, unsigned int & index)
+{
+	T max = 0;
+	index = 0;
+	if(!v || size == 0)
+	{
+		return max;
+	}
+	max = v[0];
+	for(unsigned int i=1; i<size; ++i)
+	{
+		if(max < v[i])
+		{
+			max = v[i];
+			index = i;
+		}
+	}
+
+	return max;
+}
+
+/**
+ * Get the maximum of a vector.
+ * @param v the array
+ * @param index the index of the maximum value in the vector.
+ * @return the maximum value of the array
+ */
+template<class T>
+inline T uMax(const std::vector<T> & v, unsigned int & index)
+{
+	return uMax(v.data(), v->size(), index);
+}
+
+/**
+ * Get the maximum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @return the maximum value of the array
+ */
+template<class T>
+inline T uMax(const T * v, unsigned int size)
+{
+	unsigned int index;
+	return uMax(v, size, index);
+}
+
+/**
+ * Get the maximum of a vector.
+ * @param v the array
+ * @return the maximum value of the array
+ */
+template<class T>
+inline T uMax(const std::vector<T> & v)
+{
+	return uMax(v.data(), v.size());
+}
+
+/**
+ * Get the minimum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @param index the index of the minimum value in the vector.
+ * @return the minimum value of the array
+ */
+template<class T>
+inline T uMin(const T * v, unsigned int size, unsigned int & index)
+{
+	T min = 0;
+	index = 0;
+	if(!v || size == 0)
+	{
+		return min;
+	}
+	min = v[0];
+	for(unsigned int i=1; i<size; ++i)
+	{
+		if(min > v[i])
+		{
+			min = v[i];
+			index = i;
+		}
+	}
+
+	return min;
+}
+
+/**
+ * Get the minimum of a vector.
+ * @param v the array
+ * @param index the index of the minimum value in the vector.
+ * @return the minimum value of the array
+ */
+template<class T>
+inline T uMin(const std::vector<T> & v, unsigned int & index)
+{
+	return uMin(v.data(), v.size(), index);
+}
+
+/**
+ * Get the minimum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @return the minimum value of the array
+ */
+template<class T>
+inline T uMin(const T * v, unsigned int size)
+{
+	unsigned int index;
+	return uMin(v, size, index);
+}
+
+/**
+ * Get the minimum of a vector.
+ * @param v the array
+ * @return the minimum value of the array
+ */
+template<class T>
+inline T uMin(const std::vector<T> & v)
+{
+	return uMin(v.data(), v.size());
+}
+
+/**
+ * Get the minimum and maximum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @param min reference to output minimum
+ * @param max reference to output maximum
+ * @param min reference to output minimum index
+ * @param max reference to output maximum index
+ */
+template<class T>
+inline void uMinMax(const T * v, unsigned int size, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
+{
+	min = 0;
+	max = 0;
+	indexMin = 0;
+	indexMax = 0;
+	if(!v || size == 0)
+	{
+		return;
+	}
+	min = v[0];
+	max = v[0];
+	for(unsigned int i=1; i<size; ++i)
+	{
+		if(min > v[i])
+		{
+			min = v[i];
+			indexMin = i;
+		}
+
+		if(max < v[i])
+		{
+			max = v[i];
+			indexMax = i;
+		}
+	}
+}
+
+/**
+ * Get the minimum and maximum of a vector.
+ * @param v the array
+ * @param min reference to output minimum
+ * @param max reference to output maximum
+ * @param min reference to output minimum index
+ * @param max reference to output maximum index
+ */
+template<class T>
+inline void uMinMax(const std::vector<T> & v, T & min, T & max, unsigned int & indexMin, unsigned int & indexMax)
+{
+	uMinMax(v.data(), v.size(), min, max, indexMin, indexMax);
+}
+
+/**
+ * Get the minimum and maximum of a vector.
+ * @param v the array
+ * @param size the size of the array
+ * @param min reference to output minimum
+ * @param max reference to output maximum
+ */
+template<class T>
+inline void uMinMax(const T * v, unsigned int size, T & min, T & max)
+{
+	unsigned int indexMin;
+	unsigned int indexMax;
+	uMinMax(v, size, min, max, indexMin, indexMax);
+}
+
+/**
+ * Get the minimum and maximum of a vector.
+ * @param v the array
+ * @param min reference to output minimum
+ * @param max reference to output maximum
+ */
+template<class T>
+inline void uMinMax(const std::vector<T> & v, T & min, T & max)
+{
+	uMinMax(v.data(), v.size(), min, max);
+}
+
+/**
+ * Get the sign of value.
+ * @param v the value
+ * @return -1 if v<0, otherwise 1
+ */
+template<class T>
+inline int uSign(const T & v)
+{
+	if(v < 0)
+	{
+		return -1;
+	}
+	else
+	{
+		return 1;
+	}
+}
+
+/**
+ * Get the sum of all values contained in a list. Provided for convenience.
+ * @param list the list
+ * @return the sum of values of the list
+ */
+template<class T>
+inline T uSum(const std::list<T> & list)
+{
+	T sum = 0;
+	for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
+	{
+		sum += *i;
+	}
+	return sum;
+}
+
+/**
+ * Get the sum of all values contained in an array: sum(x).
+ * @param v the array
+ * @param size the size of the array
+ * @return the sum of values of the array
+ */
+template<class T>
+inline T uSum(const T * v, unsigned int size)
+{
+	T sum = 0;
+	if(v && size)
+	{
+		for(unsigned int i=0; i<size; ++i)
+		{
+			sum += v[i];
+		}
+	}
+	return sum;
+}
+
+/**
+ * Get the sum of all values contained in a vector. Provided for convenience.
+ * @param v the vector
+ * @return the sum of values of the vector
+ */
+template<class T>
+inline T uSum(const std::vector<T> & v)
+{
+	return uSum(v.data(), v.size());
+}
+
+/**
+ * Get the sum of all squared values contained in an array: sum(x.^2).
+ * @param v the array
+ * @param size the size of the array
+ * @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
+ * @return the sum of values of the array
+ */
+template<class T>
+inline T uSumSquared(const T * v, unsigned int size, T subtract = T())
+{
+	T sum = 0;
+	if(v && size)
+	{
+		for(unsigned int i=0; i<size; ++i)
+		{
+			sum += (v[i]-subtract)*(v[i]-subtract);
+		}
+	}
+	return sum;
+}
+
+/**
+ * Get the sum of all squared values contained in an array: sum(x.^2).
+ * @param v the array
+ * @param subtract an optional value to remove to v before squaring v: sum((x-sub).^2)
+ * @return the sum of values of the array
+ */
+template<class T>
+inline T uSumSquared(const std::vector<T> & v, T subtract = T())
+{
+	return uSumSquared(v.data(), v.size(), subtract);
+}
+
+/**
+ * Compute the mean of an array.
+ * @param v the array
+ * @param size the size of the array
+ * @return the mean
+ */
+template<class T>
+inline T uMean(const T * v, unsigned int size)
+{
+	T buf = 0;
+	if(v && size)
+	{
+		for(unsigned int i=0; i<size; ++i)
+		{
+			buf += v[i];
+		}
+		buf /= size;
+	}
+	return buf;
+}
+
+/**
+ * Get the mean of a list. Provided for convenience.
+ * @param list the list
+ * @return the mean
+ */
+template<class T>
+inline T uMean(const std::list<T> & list)
+{
+	T m = 0;
+	if(list.size())
+	{
+		for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
+		{
+			m += *i;
+		}
+		m /= list.size();
+	}
+	return m;
+}
+
+/**
+ * Get the mean of a vector. Provided for convenience.
+ * @param v the vector
+ * @return the mean
+ */
+template<class T>
+inline T uMean(const std::vector<T> & v)
+{
+	return uMean(v.data(), v.size());
+}
+
+/**
+ * Compute mean squared error between two arrays: mean((x-y).^2).
+ * @param x the first array
+ * @param sizeX the size of the array x
+ * @param y the second array
+ * @param sizeY the size of the array y (must be same size as x)
+ * @return the mean squared error (return -1 if error cannot be computed)
+ */
+template<class T>
+inline T uMeanSquaredError(const T * x, unsigned int sizeX, const T * y, unsigned int sizeY)
+{
+	T sum = 0;
+	if(x && y && sizeX == sizeY)
+	{
+		for(unsigned int i=0; i<sizeX; ++i)
+		{
+			T diff = x[i]-y[i];
+			sum += diff*diff;
+		}
+		return sum/(T)sizeX;
+	}
+	return (T)-1;
+}
+
+/**
+ * Compute mean squared error between two arrays: mean((x-y).^2).
+ * @param x the first array
+ * @param y the second array (must be same size as x)
+ * @return the mean squared error (return -1 if error cannot be computed)
+ */
+template<class T>
+inline T uMeanSquaredError(const std::vector<T> & x, const std::vector<T> & y)
+{
+	return uMeanSquaredError(x.data(), x.size(), y.data(), y.size());
+}
+
+/**
+ * Compute the standard deviation of an array.
+ * @param v the array
+ * @param size the size of the array
+ * @param meanV the mean of the array
+ * @return the std dev
+ * @see mean()
+ */
+template<class T>
+inline T uStdDev(const T * v, unsigned int size, T meanV)
+{
+	T buf = 0;
+	if(v && size>1)
+	{
+		float sum = 0;
+		for(unsigned int i=0; i<size; ++i)
+		{
+			sum += (v[i]-meanV)*(v[i]-meanV);
+		}
+		buf = sqrt(sum/(size-1));
+	}
+	return buf;
+}
+
+/**
+ * Get the standard deviation of a list. Provided for convenience.
+ * @param list the list
+ * @param m the mean of the list
+ * @return the std dev
+ * @see mean()
+ */
+template<class T>
+inline T uStdDev(const std::list<T> & list, const T & m)
+{
+	T buf = 0;
+	if(list.size()>1)
+	{
+		float sum = 0;
+		for(typename std::list<T>::const_iterator i=list.begin(); i!=list.end(); ++i)
+		{
+			sum += (*i-m)*(*i-m);
+		}
+		buf = sqrt(sum/(list.size()-1));
+	}
+	return buf;
+}
+
+/**
+ * Compute the standard deviation of an array.
+ * @param v the array
+ * @param size the size of the array
+ * @return the std dev
+ */
+template<class T>
+inline T uStdDev(const T * v, unsigned int size)
+{
+	T m = uMean(v, size);
+	return uStdDev(v, size, m);
+}
+
+/**
+ * Get the standard deviation of a vector. Provided for convenience.
+ * @param v the vector
+ * @param m the mean of the vector
+ * @return the std dev
+ * @see mean()
+ */
+template<class T>
+inline T uStdDev(const std::vector<T> & v, const T & m)
+{
+	return uStdDev(v.data(), v.size(), m);
+}
+
+/**
+ * Get the norm of the vector : return sqrt(x1*x1 + x2*x2 + x3*x3 + ...)
+ * @return the norm of the vector
+ */
+template<class T>
+inline T uNorm(const std::vector<T> & v)
+{
+	float sum = 0.0f;
+	for(unsigned int i=0; i<v.size(); ++i)
+	{
+		sum += v[i]*v[i];
+	}
+	return std::sqrt(sum);
+}
+
+/**
+ * Normalize the vector : [x1 x2 x3 ...] ./ uNorm([x1 x2 x3 ...])
+ * @return the vector normalized
+ */
+template<class T>
+inline std::vector<T> uNormalize(const std::vector<T> & v)
+{
+	float norm = uNorm(v);
+	if(norm == 0)
+	{
+		return v;
+	}
+	else
+	{
+		std::vector<T> r(v.size());
+		for(unsigned int i=0; i<v.size(); ++i)
+		{
+			r[i] = v[i]/norm;
+		}
+		return r;
+	}
+}
+
+/**
+ * Find all local maxima.
+ */
+template<class T>
+inline std::list<unsigned int> uLocalMaxima(const T * v, unsigned int size)
+{
+	std::list<unsigned int> maxima;
+	if(size)
+	{
+		for(unsigned int i=0; i<size; ++i)
+		{
+			if(i == 0)
+			{
+				// first item
+				if((i+1 < size && v[i] > v[i+1]) ||
+					i+1 >= size)
+				{
+					maxima.push_back(i);
+				}
+			}
+			else if(i == size - 1)
+			{
+				//last item
+				if((i >= 1 && v[i] > v[i-1]) ||
+					i == 0)
+				{
+					maxima.push_back(i);
+				}
+			}
+			else
+			{
+				//all others, check previous and next
+				if(v[i] > v[i-1] && v[i] > v[i+1])
+				{
+					maxima.push_back(i);
+				}
+			}
+		}
+	}
+	return maxima;
+}
+
+/**
+ * Find all local maxima.
+ */
+template<class T>
+inline std::list<unsigned int> uLocalMaxima(const std::vector<T> & v)
+{
+	return uLocalMaxima(v.data(), v.size());
+}
+
+/**
+ * Enum of cross matching methods (cross-correlation, cross-covariance) :
+ * UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff.
+ */
+enum UXMatchMethod{UXCorrRaw, UXCorrBiased, UXCorrUnbiased, UXCorrCoeff, UXCovRaw, UXCovBiased, UXCovUnbiased, UXCovCoeff};
+
+/**
+ * Do a full cross-correlation or cross-covariance between 2 arrays.
+ * @param vA the first array
+ * @param vB the second array
+ * @param sizeA the size of the first array
+ * @param sizeB the size of the second array
+ * @param method see UXMatchMethod
+ * @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
+ */
+template<class T>
+inline std::vector<T> uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, UXMatchMethod method)
+{
+	if(!vA || !vB || sizeA == 0 || sizeB == 0)
+	{
+		return std::vector<T>();
+	}
+
+	std::vector<T> result(sizeA + sizeB - 1);
+
+	T meanA = 0;
+	T meanB = 0;
+	if(method > UXCorrCoeff)
+	{
+		meanA = uMean(vA, sizeA);
+		meanB = uMean(vB, sizeB);
+	}
+
+	T den = 1;
+	if(method == UXCorrCoeff || method == UXCovCoeff)
+	{
+		den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
+	}
+	else if(method == UXCorrBiased || method == UXCovBiased)
+	{
+		den = std::max(sizeA, sizeB);
+	}
+
+	if(sizeA == sizeB)
+	{
+		T resultA;
+		T resultB;
+
+		int posA;
+		int posB;
+		unsigned int j;
+
+		// Optimization, filling two results at once
+		for(unsigned int i=0; i<sizeA; ++i)
+		{
+			if(method == UXCorrUnbiased || method == UXCovUnbiased)
+			{
+				den = 0;
+			}
+
+			posA = sizeA - i - 1;
+			posB = sizeB - i - 1;
+			resultA = 0;
+			resultB = 0;
+			for(j=0; (j + posB) < sizeB && (j + posA) < sizeA; ++j)
+			{
+				resultA += (vA[j] - meanA) * (vB[j + posB] - meanB);
+				resultB += (vA[j + posA] - meanA) * (vB[j] - meanB);
+				if(method == UXCorrUnbiased || method == UXCovUnbiased)
+				{
+					++den;
+				}
+			}
+
+			result[i] = resultA / den;
+			result[result.size()-1 -i] = resultB / den;
+		}
+	}
+	else
+	{
+		for(unsigned int i=0; i<result.size(); ++i)
+		{
+			if(method == UXCorrUnbiased || method == UXCovUnbiased)
+			{
+				den = 0;
+			}
+
+			int posB = sizeB - i - 1;
+			T r = 0;
+			if(posB >= 0)
+			{
+				for(unsigned int j=0; (j + posB) < sizeB && j < sizeA; ++j)
+				{
+					r += (vA[j] - meanA) * (vB[j + posB] - meanB);
+					if(method == UXCorrUnbiased || method == UXCovUnbiased)
+					{
+						++den;
+					}
+				}
+			}
+			else
+			{
+				int posA = posB*-1;
+				for(unsigned int i=0; (i+posA) < sizeA && i < sizeB; ++i)
+				{
+					r += (vA[i+posA] - meanA) * (vB[i] - meanB);
+					if(method == UXCorrUnbiased || method == UXCovUnbiased)
+					{
+						++den;
+					}
+				}
+			}
+
+			result[i] = r / den;
+		}
+	}
+
+	return result;
+}
+
+/**
+ * Do a full cross-correlation or cross-covariance between 2 arrays.
+ * @param vA the first array
+ * @param vB the second array
+ * @param method see UXMatchMethod
+ * @return the resulting correlation/covariance vector of size = sizeA + sizeB - 1
+ */
+template<class T>
+inline std::vector<T> uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, UXMatchMethod method)
+{
+	return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), method);
+}
+
+/**
+ * Do a cross correlation between 2 arrays at a specified index.
+ * @param vA the first array
+ * @param vB the second array
+ * @param sizeA the size of the first array
+ * @param sizeB the size of the second array
+ * @param index the index to correlate
+ * @param method see UXMatchMethod
+ * @return the resulting correlation value
+ */
+template<class T>
+inline T uXMatch(const T * vA, const T * vB, unsigned int sizeA, unsigned int sizeB, unsigned int index, UXMatchMethod method)
+{
+	T result = 0;
+	if(!vA || !vB || sizeA == 0 || sizeB == 0)
+	{
+		return result;
+	}
+
+	T meanA = 0;
+	T meanB = 0;
+	if(method > UXCorrCoeff)
+	{
+		meanA = uMean(vA, sizeA);
+		meanB = uMean(vB, sizeB);
+	}
+	unsigned int size = sizeA + sizeB - 1;
+
+	T den = 1;
+	if(method == UXCorrCoeff || method == UXCovCoeff)
+	{
+		den = std::sqrt(uSumSquared(vA, sizeA, meanA) * uSumSquared(vB, sizeB, meanB));
+	}
+	else if(method == UXCorrBiased || method == UXCovBiased)
+	{
+		den = std::max(sizeA, sizeB);
+	}
+	else if(method == UXCorrUnbiased || method == UXCovUnbiased)
+	{
+		den = 0;
+	}
+
+	if(index < size)
+	{
+		int posB = sizeB - index - 1;
+		unsigned int i;
+		if(posB >= 0)
+		{
+			for(i=0; (i + posB) < sizeB && i < sizeA; ++i)
+			{
+				result += (vA[i] - meanA) * (vB[i + posB] - meanB);
+				if(method == UXCorrUnbiased || method == UXCovUnbiased)
+				{
+					++den;
+				}
+			}
+		}
+		else
+		{
+			int posA = posB*-1;
+			for(i=0; (i+posA) < sizeA && i < sizeB; ++i)
+			{
+				result += (vA[i+posA] - meanA) * (vB[i] - meanB);
+				if(method == UXCorrUnbiased || method == UXCovUnbiased)
+				{
+					++den;
+				}
+			}
+		}
+	}
+	return result / den;
+}
+
+/**
+ * Do a cross correlation between 2 arrays at a specified index.
+ * @param vA the first array
+ * @param vB the second array
+ * @param sizeA the size of the first array
+ * @param sizeB the size of the second array
+ * @param index the index to correlate
+ * @param method see UXMatchMethod
+ * @return the resulting correlation value
+ */
+template<class T>
+inline T uXMatch(const std::vector<T> & vA, const std::vector<T> & vB, unsigned int index, UXMatchMethod method)
+{
+	return uXMatch(vA.data(), vB.data(), vA.size(), vB.size(), index, method);
+}
+
+/**
+ * Return Hamming window of length L.
+ * @param L the window length
+ * @return the Hamming window (values are between 0 and 1)
+ */
+inline std::vector<float> uHamming(unsigned int L)
+{
+	std::vector<float> w(L);
+	unsigned int N = L-1;
+	float pi = 3.14159265f;
+	for(unsigned int n=0; n<N; ++n)
+	{
+		w[n] = 0.54-0.46*std::cos(2*pi*n/N);
+	}
+	return w;
+}
+
+#endif // UMATH_H

utilite/include/rtabmap/utilite/UMutex.h

@@ -0,0 +1,179 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UMUTEX_H
+#define UMUTEX_H
+
+#include <errno.h>
+
+#ifdef WIN32
+  #include "rtabmap/utilite/Win32/UWin32.h"
+#else
+  #include <pthread.h>
+#endif
+
+
+/**
+ * A mutex class.
+ *
+ * On a lock() call, the calling thread is blocked if the
+ * UMutex was previously locked by another thread. It is unblocked when unlock() is called.
+ *
+ * On Unix (not yet tested on Windows), UMutex is recursive: the same thread can
+ * call multiple times lock() without being blocked.
+ *
+ * Example:
+ * @code
+ * UMutex m; // Mutex shared with another thread(s).
+ * ...
+ * m.lock();
+ * // Data is protected here from the second thread
+ * //(assuming the second one protects also with the same mutex the same data).
+ * m.unlock();
+ *
+ * @endcode
+ *
+ * @see USemaphore
+ */
+class UMutex
+{
+
+public:
+
+	/**
+	 * The constructor.
+	 */
+	UMutex()
+	{
+#ifdef WIN32
+		InitializeCriticalSection(&C);
+#else
+		pthread_mutexattr_t attr;
+		pthread_mutexattr_init(&attr);
+		pthread_mutexattr_settype(&attr,PTHREAD_MUTEX_RECURSIVE);
+		pthread_mutex_init(&M,&attr);
+		pthread_mutexattr_destroy(&attr);
+#endif
+	}
+
+	virtual ~UMutex()
+	{
+#ifdef WIN32
+		DeleteCriticalSection(&C);
+#else
+		pthread_mutex_unlock(&M); pthread_mutex_destroy(&M);
+#endif
+	}
+
+	/**
+	 * Lock the mutex.
+	 */
+	int lock() const
+	{
+#ifdef WIN32
+		EnterCriticalSection(&C); return 0;
+#else
+		return pthread_mutex_lock(&M);
+#endif
+	}
+
+#ifdef WIN32
+	#if(_WIN32_WINNT >= 0x0400)
+	int lockTry() const
+	{
+		return (TryEnterCriticalSection(&C)?0:EBUSY);
+	}
+	#endif
+#else
+	int lockTry() const
+	{
+		return pthread_mutex_trylock(&M);
+	}
+#endif
+
+	/**
+	 * Unlock the mutex.
+	 */
+	int unlock() const
+	{
+#ifdef WIN32
+		LeaveCriticalSection(&C); return 0;
+#else
+		return pthread_mutex_unlock(&M);
+#endif
+	}
+
+	private:
+#ifdef WIN32
+		mutable CRITICAL_SECTION C;
+#else
+		mutable pthread_mutex_t M;
+#endif
+		void operator=(UMutex &M) {}
+		UMutex( const UMutex &M ) {}
+};
+
+/**
+ * Automatically lock the referenced mutex on constructor and unlock mutex on destructor.
+ *
+ * Example:
+ * @code
+ * UMutex m; // Mutex shared with another thread(s).
+ * ...
+ * int myMethod()
+ * {
+ *    UScopeMutex sm(m); // automatically lock the mutex m
+ *    if(cond1)
+ *    {
+ *       return 1; // automatically unlock the mutex m
+ *    }
+ *    else if(cond2)
+ *    {
+ *       return 2; // automatically unlock the mutex m
+ *    }
+ *    return 0; // automatically unlock the mutex m
+ * }
+ *
+ * @endcode
+ *
+ * @see UMutex
+ */
+class UScopeMutex
+{
+public:
+	UScopeMutex(const UMutex & mutex) :
+		mutex_(mutex)
+	{
+		mutex_.lock();
+	}
+	// backward compatibility
+	UScopeMutex(UMutex * mutex) :
+		mutex_(*mutex)
+	{
+		mutex_.lock();
+	}
+	~UScopeMutex()
+	{
+		mutex_.unlock();
+	}
+private:
+	const UMutex & mutex_;
+};
+
+#endif // UMUTEX_H

utilite/include/rtabmap/utilite/UObjDeletionThread.h

@@ -0,0 +1,129 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UOBJDELETIONTHREAD_H
+#define UOBJDELETIONTHREAD_H
+
+#include "rtabmap/utilite/UThreadNode.h"
+#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/UEventsManager.h"
+
+/**
+ * Event used by UObjDeletionThread to notify when its object is deleted. It contains
+ * the object id used for deletion (can be retrieved by UEvent::getCode()).
+ */
+class UObjDeletedEvent : public UEvent
+{
+public:
+	UObjDeletedEvent(int objDeletionThreadId) : UEvent(objDeletionThreadId) {}
+	virtual ~UObjDeletedEvent() {}
+	/**
+	 * @return string "UObjDeletedEvent"
+	 */
+	virtual std::string getClassName() const {return std::string("UObjDeletedEvent");}
+};
+
+/**
+ * This class can be used to delete a dynamically created object in another thread. Give the
+ * dynamic reference to object to it and it will notify with a UObjDeletedEvent when the object is deleted.
+ * The deletion can be delayed on startDeletion(), the thread will wait the time given before deleting the object.
+ */
+template<class T>
+class UObjDeletionThread : public UThread
+{
+public:
+	/**
+	 * The constructor.
+	 * @param obj the object to delete
+	 * @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
+	 */
+	UObjDeletionThread(T * obj, int id=0) :
+		obj_(obj),
+		id_(id),
+		waitMs_(0) {}
+
+	/**
+	 * The destructor. If this thread is not started but with an object set, the
+	 * object is deleted. If the thread has not finished to delete the object, the
+	 * calling thread will wait (on a UThreadNode::join()) until the object is deleted.
+	 * @param obj the object to delete
+	 * @param id the custom id which will be sent in a event UObjDeletedEvent after the object is deleted
+	 */
+	virtual ~UObjDeletionThread()
+	{
+		join(true);
+		if(obj_)
+		{
+			delete obj_;
+		}
+	}
+
+	/**
+	 * Start the thread after optional delay.
+	 * @param waitMs the delay before deletion
+	 */
+	void startDeletion(int waitMs = 0) {waitMs_ = waitMs; this->start();}
+
+	/**
+	 * Get id of the deleted object.
+	 * @return the id
+	 */
+	int id() const {return id_;}
+
+	/**
+	 * Set a new object, if one was already set, the old one is deleted.
+	 * @param obj the object to delete
+	 */
+	void setObj(T * obj)
+	{
+		join();
+		if(obj_)
+		{
+			delete obj_;
+			obj_ = 0;
+		}
+		obj_ = obj;
+	}
+
+private:
+	/**
+	 * Thread main loop...
+	 */
+	virtual void mainLoop()
+	{
+		if(waitMs_)
+		{
+			uSleep(waitMs_);
+		}
+		if(obj_)
+		{
+			delete obj_;
+			obj_ = 0;
+		}
+		this->kill();
+		UEventsManager::post(new UObjDeletedEvent(id_), false);
+	}
+
+private:
+	T * obj_;
+	int id_;
+	int waitMs_;
+};
+
+#endif /* UOBJDELETIONTHREAD_H */

utilite/include/rtabmap/utilite/UProcessInfo.h

@@ -0,0 +1,41 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UPROCESSINFO_H
+#define UPROCESSINFO_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+/**
+ * This class is used to get some informations
+ * about the current process.
+ */
+class UTILITE_EXP UProcessInfo {
+public:
+	UProcessInfo();
+	virtual ~UProcessInfo();
+
+	/**
+	 * Get the memory used by the current process.
+	 * @return the number of bytes used by the current process.
+	 */
+	static long int getMemoryUsage();
+};
+
+#endif /* UPROCESSINFO_H */

utilite/include/rtabmap/utilite/USemaphore.h

@@ -0,0 +1,196 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*
+ * Originally written by Phillip Sitbon
+ *  Copyright 2003
+ */
+
+#ifndef USEMAPHORE_H
+#define USEMAPHORE_H
+
+#include <errno.h>
+
+#ifdef WIN32
+#include "rtabmap/utilite/Win32/UWin32.h"
+#define SEM_VALUE_MAX ((int) ((~0u) >> 1))
+#else
+#include <pthread.h>
+#endif
+
+/**
+ * A semaphore class.
+ *
+ * On an acquire() call, the calling thread is blocked if the
+ * USemaphore's value is <= 0. It is unblocked when release() is called.
+ * The function acquire() decreases by 1 (default) the
+ * semaphore's value and release() increases it by 1 (default).
+ *
+ * Example:
+ * @code
+ * USemaphore s;
+ * s.acquire(); // Will wait until s.release() is called by another thread.
+ * @endcode
+ *
+ * @see UMutex
+ */
+class USemaphore
+{
+public:
+	/**
+	 * The constructor. The semaphore waits on acquire() when its value is <= 0.
+	 * @param n number to initialize
+	 */
+	USemaphore( int initValue = 0 )
+	{
+#ifdef WIN32
+		S = CreateSemaphore(0,initValue,SEM_VALUE_MAX,0);
+#else
+		_available = initValue;
+		pthread_mutex_init(&_waitMutex, NULL);
+		pthread_cond_init(&_cond, NULL);
+#endif
+	}
+
+	virtual ~USemaphore()
+	{
+#ifdef WIN32
+		CloseHandle(S);
+#else
+		pthread_cond_destroy(&_cond);
+		pthread_mutex_destroy(&_waitMutex);
+#endif
+	}
+
+	/**
+	 * Acquire the semaphore. If semaphore's value is <=0, the
+	 * calling thread will wait until the count acquired is released.
+	 * @see release()
+	 * @param n number to acquire
+	 */
+#ifdef WIN32
+	void acquire(int n = 1) const
+	{
+		while(n-- > 0)
+		{
+			WaitForSingleObject((HANDLE)S,INFINITE);
+		}
+#else
+	void acquire(int n = 1)
+	{
+		pthread_mutex_lock(&_waitMutex);
+		while (n > _available)
+		{
+			while(1)
+			{
+				pthread_cond_wait(&_cond, &_waitMutex);
+				break;
+			}
+		}
+		_available -= n;
+		pthread_mutex_unlock(&_waitMutex);
+#endif
+	}
+
+	/*
+	 * Try to acquire the semaphore, not a blocking call.
+	 * @return false if the semaphore can't be taken without waiting (value <= 0), true otherwise
+	 */
+#ifdef WIN32
+	int acquireTry() const
+	{
+		return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN);
+#else
+	int acquireTry(int n)
+	{
+		pthread_mutex_lock(&_waitMutex);
+		if(n > _available)
+		{
+			pthread_mutex_unlock(&_waitMutex);
+			return false;
+		}
+		_available -= n;
+		pthread_mutex_unlock(&_waitMutex);
+		return true;
+#endif
+	}
+
+	/**
+	 * Release the semaphore, increasing its value by 1 and
+	 * signaling waiting threads (which called acquire()).
+	 */
+#ifdef WIN32
+	int release(int n = 1) const
+	{
+		return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE);
+#else
+	void release(int n = 1)
+	{
+		pthread_mutex_lock(&_waitMutex);
+		_available += n;
+		pthread_cond_broadcast(&_cond);
+		pthread_mutex_unlock(&_waitMutex);
+#endif
+	}
+
+	/**
+	 * Get the USempahore's value.
+	 * @return the semaphore's value
+	 */
+#ifdef WIN32
+	int value() const
+	{
+		LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V;
+#else
+	int value()
+	{
+		int value = 0;
+		pthread_mutex_lock(&_waitMutex);
+		value = _available;
+		pthread_mutex_unlock(&_waitMutex);
+		return value;
+#endif
+	}
+
+#ifdef WIN32
+	/*
+	 * Reset the semaphore count.
+	 * @param init the initial value
+	 * TODO implement on posix ?
+	 */
+	void reset( int init = 0 )
+	{
+		CloseHandle(S);
+		S = CreateSemaphore(0,init,SEM_VALUE_MAX,0);
+	}
+#endif
+
+private:
+#ifdef WIN32
+	HANDLE S;
+#else
+	pthread_mutex_t _waitMutex;
+	pthread_cond_t _cond;
+	int _available;
+#endif
+	void operator=(const USemaphore &S){}
+	USemaphore(const USemaphore &S){}
+};
+
+#endif // USEMAPHORE_H

utilite/include/rtabmap/utilite/UStl.h

@@ -0,0 +1,625 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef USTL_H
+#define USTL_H
+
+#include <list>
+#include <map>
+#include <set>
+#include <vector>
+#include <string>
+#include <algorithm>
+
+/**
+ * \file UStl.h
+ * \brief Wrappers of STL for convenient functions.
+ *
+ * All functions you will find here are here
+ * for the use of STL in a more convenient way.
+ */
+
+
+/**
+ * Get unique keys from a std::multimap.
+ * @param mm the multimap
+ * @return the list which contains unique keys
+ */
+template<class K, class V>
+inline std::list<K> uUniqueKeys(const std::multimap<K, V> & mm)
+{
+	std::list<K> l;
+	typename std::list<K>::reverse_iterator lastValue;
+	for(typename std::multimap<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
+	{
+		if(iter == mm.begin() || (iter != mm.begin() && *lastValue != iter->first))
+		{
+			l.push_back(iter->first);
+			lastValue = l.rbegin();
+		}
+	}
+	return l;
+}
+
+/**
+ * Get all keys from a std::multimap.
+ * @param mm the multimap
+ * @return the vector which contains all keys (may contains duplicated keys)
+ */
+template<class K, class V>
+inline std::vector<K> uKeys(const std::multimap<K, V> & mm)
+{
+	std::vector<K> v(mm.size());
+	int i=0;
+	for(typename std::map<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
+	{
+		v[i++] = iter->first;
+	}
+	return v;
+}
+
+/**
+ * Get all keys from a std::multimap.
+ * @param mm the multimap
+ * @return the list which contains all keys (may contains duplicated keys)
+ */
+template<class K, class V>
+inline std::list<K> uKeysList(const std::multimap<K, V> & mm)
+{
+	std::list<K> l;
+	for(typename std::map<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
+	{
+		l.push_back(iter->first);
+	}
+	return l;
+}
+
+/**
+ * Get all values from a std::multimap.
+ * @param mm the multimap
+ * @return the vector which contains all values (contains values from duplicated keys)
+ */
+template<class K, class V>
+inline std::vector<V> uValues(const std::multimap<K, V> & mm)
+{
+	std::vector<V> v(mm.size());
+	int i=0;
+	for(typename std::map<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
+	{
+		v[i++] = iter->second;
+	}
+	return v;
+}
+
+/**
+ * Get all values from a std::multimap.
+ * @param mm the multimap
+ * @return the list which contains all values (contains values from duplicated keys)
+ */
+template<class K, class V>
+inline std::list<V> uValuesList(const std::multimap<K, V> & mm)
+{
+	std::list<V> l;
+	for(typename std::map<K, V>::const_iterator iter = mm.begin(); iter!=mm.end(); ++iter)
+	{
+		l.push_back(iter->second);
+	}
+	return l;
+}
+
+/**
+ * Get values for a specified key from a std::multimap.
+ * @param mm the multimap
+ * @param key the key
+ * @return the list which contains the values of the key
+ */
+template<class K, class V>
+inline std::list<V> uValues(const std::multimap<K, V> & mm, const K & key)
+{
+	std::list<V> l;
+	std::pair<typename std::multimap<K, V>::const_iterator, typename std::multimap<K, V>::const_iterator> range;
+	range = mm.equal_range(key);
+	for(typename std::multimap<K, V>::const_iterator iter = range.first; iter!=range.second; ++iter)
+	{
+		l.push_back(iter->second);
+	}
+	return l;
+}
+
+/**
+ * Get all keys from a std::map.
+ * @param m the map
+ * @return the vector of keys
+ */
+template<class K, class V>
+inline std::vector<K> uKeys(const std::map<K, V> & m)
+{
+	std::vector<K> v(m.size());
+	int i=0;
+	for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
+	{
+		v[i] = iter->first;
+		++i;
+	}
+	return v;
+}
+
+/**
+ * Get all keys from a std::map.
+ * @param m the map
+ * @return the list of keys
+ */
+template<class K, class V>
+inline std::list<K> uKeysList(const std::map<K, V> & m)
+{
+	std::list<K> l;
+	for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
+	{
+		l.push_back(iter->first);
+	}
+	return l;
+}
+
+/**
+ * Get all keys from a std::map.
+ * @param m the map
+ * @return the set of keys
+ */
+template<class K, class V>
+inline std::set<K> uKeysSet(const std::map<K, V> & m)
+{
+	std::set<K> s;
+	int i=0;
+	for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
+	{
+		s.insert(s.end(), iter->first);
+		++i;
+	}
+	return s;
+}
+
+/**
+ * Get all values from a std::map.
+ * @param m the map
+ * @return the vector of values
+ */
+template<class K, class V>
+inline std::vector<V> uValues(const std::map<K, V> & m)
+{
+	std::vector<V> v(m.size());
+	int i=0;
+	for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
+	{
+		v[i] = iter->second;
+		++i;
+	}
+	return v;
+}
+
+/**
+ * Get all values from a std::map.
+ * @param m the map
+ * @return the list of values
+ */
+template<class K, class V>
+inline std::list<V> uValuesList(const std::map<K, V> & m)
+{
+	std::list<V> l;
+	for(typename std::map<K, V>::const_iterator iter = m.begin(); iter!=m.end(); ++iter)
+	{
+		l.push_back(iter->second);
+	}
+	return l;
+}
+
+/**
+ * Get the value of a specified key from a std::map.
+ * @param m the map
+ * @param key the key
+ * @param defaultValue the default value used if the key is not found
+ * @return the value
+ */
+template<class K, class V>
+inline V uValue(const std::map<K, V> & m, const K & key, const V & defaultValue = V())
+{
+	V v = defaultValue;
+	typename std::map<K, V>::const_iterator i = m.find(key);
+	if(i != m.end())
+	{
+		v = i->second;
+	}
+	return v;
+}
+
+/**
+ * Get the value of a specified key from a std::map. This will
+ * remove the value from the map;
+ * @param m the map
+ * @param key the key
+ * @param defaultValue the default value used if the key is not found
+ * @return the value
+ */
+template<class K, class V>
+inline V uTake(std::map<K, V> & m, const K & key, const V & defaultValue = V())
+{
+	V v;
+	typename std::map<K, V>::iterator i = m.find(key);
+	if(i != m.end())
+	{
+		v = i->second;
+		m.erase(i);
+	}
+	else
+	{
+		v = defaultValue;
+	}
+	return v;
+}
+
+/**
+ * Get the iterator at a specified position in a std::list. If the position
+ * is out of range, the result is the end iterator of the list.
+ * @param list the list
+ * @param pos the index position in the list
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::list<V>::iterator uIteratorAt(std::list<V> & list, const unsigned int & pos)
+{
+	typename std::list<V>::iterator iter = list.begin();
+	for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
+	{
+		++iter;
+	}
+	return iter;
+}
+
+/**
+ * Get the iterator at a specified position in a std::list. If the position
+ * is out of range, the result is the end iterator of the list.
+ * @param list the list
+ * @param pos the index position in the list
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::list<V>::const_iterator uIteratorAt(const std::list<V> & list, const unsigned int & pos)
+{
+	typename std::list<V>::const_iterator iter = list.begin();
+	for(unsigned int i = 0; i<pos && iter != list.end(); ++i )
+	{
+		++iter;
+	}
+	return iter;
+}
+
+/**
+ * Get the iterator at a specified position in a std::vector. If the position
+ * is out of range, the result is the end iterator of the vector.
+ * @param v the vector
+ * @param pos the index position in the vector
+ * @return the iterator at the specified index
+ */
+template<class V>
+inline typename std::vector<V>::iterator uIteratorAt(std::vector<V> & v, const unsigned int & pos)
+{
+	return v.begin() + pos;
+}
+
+/**
+ * Get the value at a specified position in a std::list. If the position
+ * is out of range, the result is undefined.
+ * @param list the list
+ * @param pos the index position in the list
+ * @return the value at the specified index
+ */
+template<class V>
+inline V & uValueAt(std::list<V> & list, const unsigned int & pos)
+{
+	typename std::list<V>::iterator iter = uIteratorAt(list, pos);
+	return *iter;
+}
+
+/**
+ * Get the value at a specified position in a std::list. If the position
+ * is out of range, the result is undefined.
+ * @param list the list
+ * @param pos the index position in the list
+ * @return the value at the specified index
+ */
+template<class V>
+inline const V & uValueAt(const std::list<V> & list, const unsigned int & pos)
+{
+	typename std::list<V>::const_iterator iter = uIteratorAt(list, pos);
+	return *iter;
+}
+
+/**
+ * Check if the list contains the specified value.
+ * @param list the list
+ * @param value the value
+ * @return true if the value is found in the list, otherwise false
+ */
+template<class V>
+inline bool uContains(const std::list<V> & list, const V & value)
+{
+	return std::find(list.begin(), list.end(), value) != list.end();
+}
+
+/**
+ * Check if the map contains the specified key.
+ * @param map the map
+ * @param key the key
+ * @return true if the value is found in the map, otherwise false
+ */
+template<class K, class V>
+inline bool uContains(const std::map<K, V> & map, const K & key)
+{
+	return map.find(key) != map.end();
+}
+
+/**
+ * Check if the multimap contains the specified key.
+ * @param map the map
+ * @param key the key
+ * @return true if the value is found in the map, otherwise false
+ */
+template<class K, class V>
+inline bool uContains(const std::multimap<K, V> & map, const K & key)
+{
+	return map.find(key) != map.end();
+}
+
+/**
+ * Insert an item in the map. Contrary to the insert in the STL,
+ * if the key already exists, the value will be replaced by the new one.
+ */
+template<class K, class V>
+inline void uInsert(std::map<K, V> & map, const std::pair<K, V> & pair)
+{
+	std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(pair);
+	if(inserted.second == false)
+	{
+		inserted.first->second = pair.second;
+	}
+}
+
+/**
+ * Convert a std::list to a std::vector.
+ * @param list the list
+ * @return the vector
+ */
+template<class V>
+inline std::vector<V> uListToVector(const std::list<V> & list)
+{
+	return std::vector<V>(list.begin(), list.end());
+}
+
+/**
+ * Convert a std::vector to a std::list.
+ * @param v the vector
+ * @return the list
+ */
+template<class V>
+inline std::list<V> uVectorToList(const std::vector<V> & v)
+{
+	return std::list<V>(v.begin(), v.end());
+}
+
+/**
+ * Append a list to another list.
+ * @param list the list on which the other list will be appended
+ * @param newItems the list of items to be appended
+ */
+template<class V>
+inline void uAppend(std::list<V> & list, const std::list<V> & newItems)
+{
+	list.insert(list.end(), newItems.begin(), newItems.end());
+}
+
+/**
+ * Get the index in the list of the specified value. S negative index is returned
+ * if the value is not found.
+ * @param list the list
+ * @param value the value
+ * @return the index of the value in the list
+ */
+template<class V>
+inline int uIndexOf(const std::vector<V> & list, const V & value)
+{
+	int index=-1;
+	int i=0;
+	for(typename std::vector<V>::const_iterator iter = list.begin(); iter!=list.end(); ++iter)
+	{
+		if(*iter == value)
+		{
+			index = i;
+			break;
+		}
+		++i;
+	}
+	return index;
+}
+
+/**
+ * Split a string into multiple string around the specified separator.
+ * Example:
+ * @code
+ * 		std::list<std::string> v = split("Hello the world!", ' ');
+ * @endcode
+ * The list v will contain {"Hello", "the", "world!"}
+ * @param str the string
+ * @param separator the separator character
+ * @return the list of strings
+ */
+inline std::list<std::string> uSplit(const std::string & str, char separator = ' ')
+{
+	std::list<std::string> v;
+	std::string buf;
+	for(unsigned int i=0; i<str.size(); ++i)
+	{
+		if(str[i] != separator)
+		{
+			buf += str[i];
+		}
+		else if(buf.size())
+		{
+			v.push_back(buf);
+			buf = "";
+		}
+	}
+	if(buf.size())
+	{
+		v.push_back(buf);
+	}
+	return v;
+}
+
+/**
+ * Check if a character is a digit.
+ * @param c the character
+ * @return if the character is a digit (if c >= '0' && c <= '9')
+ */
+inline bool uIsDigit(const char c)
+{
+	return c >= '0' && c <= '9';
+}
+
+/**
+ * Split a string into number and character strings.
+ * Example:
+ * @code
+ * 		std::list<std::string> v = uSplit("Hello 03 my 65 world!");
+ * @endcode
+ * The list v will contain {"Hello ", "03", " my ", "65", " world!"}
+ * @param str the string
+ * @return the list of strings
+ */
+inline std::list<std::string> uSplitNumChar(const std::string & str)
+{
+	std::list<std::string> list;
+	std::string buf;
+	bool num = false;
+	for(unsigned int i=0; i<str.size(); ++i)
+	{
+		if(uIsDigit(str[i]))
+		{
+			if(!num && buf.size())
+			{
+				list.push_back(buf);
+				buf.clear();
+			}
+			buf += str[i];
+			num = true;
+		}
+		else
+		{
+			if(num)
+			{
+				list.push_back(buf);
+				buf.clear();
+			}
+			buf += str[i];
+			num = false;
+		}
+	}
+	if(buf.size())
+	{
+		list.push_back(buf);
+	}
+	return list;
+}
+
+/**
+ * Compare two alphanumeric strings. Useful to sort filenames (human-like sorting).
+ * Example:
+ * @code
+ * 	std::string a = "Image9.jpg";
+ * 	std::string b = "Image10.jpg";
+ * 	int r = uStrNumCmp(a, b); // r returns -1 (a is smaller than b). In contrast, std::strcmp(a, b) would return 1.
+ * @endcode
+ * @param a the first string
+ * @param b the second string
+ * @return -1 if a<b, 0 if a=b and 1 if a>b
+ */
+inline int uStrNumCmp(const std::string & a, const std::string & b)
+{
+	std::vector<std::string> listA;
+	std::vector<std::string> listB;
+
+	listA = uListToVector(uSplitNumChar(a));
+	listB = uListToVector(uSplitNumChar(b));
+
+	unsigned int i;
+	int result = 0;
+	for(i=0; i<listA.size() && i<listB.size(); ++i)
+	{
+		if(uIsDigit(listA[i].at(0)) && uIsDigit(listB[i].at(0)))
+		{
+			//padding if zeros at the beginning
+			if(listA[i].at(0) == '0' && listB[i].size() < listA[i].size())
+			{
+				while(listB[i].size() < listA[i].size())
+				{
+					listB[i] += '0';
+				}
+			}
+			else if(listB[i].at(0) == '0' && listA[i].size() < listB[i].size())
+			{
+				while(listA[i].size() < listB[i].size())
+				{
+					listA[i] += '0';
+				}
+			}
+
+			if(listB[i].size() < listA[i].size())
+			{
+				result = 1;
+			}
+			else if(listB[i].size() > listA[i].size())
+			{
+				result = -1;
+			}
+			else
+			{
+				result = listA[i].compare(listB[i]);
+			}
+		}
+		else if(uIsDigit(listA[i].at(0)))
+		{
+			result = -1;
+		}
+		else if(uIsDigit(listB[i].at(0)))
+		{
+			result = 1;
+		}
+		else
+		{
+			result = listA[i].compare(listB[i]);
+		}
+
+		if(result != 0)
+		{
+			break;
+		}
+	}
+
+	return result;
+}
+
+#endif /* USTL_H */

utilite/include/rtabmap/utilite/UThread.h

@@ -0,0 +1,268 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTHREADNODE_H
+#define UTHREADNODE_H
+
+#include "rtabmap/utilite/UtiLiteExp.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UThreadC.h"
+
+/**
+ * The class UThread is an abstract class for creating thread objects.
+ * A UThread provides methods to create threads as an object-style fashion.
+ *
+ * For most of inherited classes, only mainLoop() needs to be implemented, then only start() needs
+ * to be called from the outside.
+ * The main loop is called until the thread itself calls kill() or another thread
+ * calls kill() or join() (with parameter to true) on this thread. Unlike kill(), join() is a blocking call:
+ * the calling thread will wait until this thread has finished, thus join() must not be
+ * called inside the mainLoop().
+ *
+ * If inside the mainLoop(), at some time, the thread needs to wait on a mutex/semaphore
+ * (like for the acquisition of a resource), the function mainLoopKill() should be
+ * implemented to release the mutex/semaphore when the thread is killed, to avoid a deadlock.
+ * The function killCleanup() is called after the thread's state is set to kSKilled.
+ * After the mutex/semaphore is released in killCleanup(), on wake up, the thread can know if
+ * it needs to stop by calling isKilled().
+ *
+ * To do an initialization process (executed by the worker thread) just one time before
+ * entering the mainLoop(), mainLoopBegin() can be implemented.
+ *
+ * Example:
+ * @code
+ * #include "utilite/UThread.h"
+ * class SimpleThread : public UThread
+ * {
+ * public:
+ * 	SimpleThread() {}
+ * 	virtual ~SimpleThread() {
+ * 		// The calling thread will wait until this thread has finished.
+ * 		this->join(true);
+ *	}
+ *
+ * protected:
+ * 	virtual void mainLoop() {
+ * 		// Do some works...
+ *
+ * 		// This will stop the thread, otherwise the mainLoop() is recalled.
+ *		this->kill();
+ * 	}
+ * };
+ *
+ * int main(int argc, char * argv[])
+ * {
+ *	SimpleThread t;
+ *	t.start();
+ *	t.join(); // Wait until the thread has finished.
+ * 	return 0;
+ * }
+ * @endcode
+ *
+ * @see start()
+ * @see kill()
+ * @see join()
+ * @see mainLoopBegin()
+ * @see mainLoopKill()
+ * @see mainLoop()
+ *
+ */
+class UTILITE_EXP UThread : public UThreadC<void>
+{
+public:
+    /**
+     * Enum of priorities : kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime.
+     */
+    enum Priority{kPLow, kPBelowNormal, kPNormal, kPAboveNormal, kPRealTime};
+
+public:
+    /**
+     * The constructor.
+     * @see Priority
+     * @param priority the thread priority
+     */
+    UThread(Priority priority = kPNormal);
+
+    /**
+     * The destructor. Inherited classes must call join(true) inside their destructor
+     * to avoid memory leaks where the underlying c-thread is still running.
+     *
+     * Note: not safe to delete a thread while other threads are joining it.
+     */
+    virtual ~UThread();
+
+    /**
+     * Start the thread. Once the thread is started, subsequent calls
+     * to start() are ignored until the thread is killed.
+     * @see kill()
+     */
+    void start();
+
+    /**
+	 * Kill the thread.
+	 * This functions does nothing if the thread is not started or is killed.
+	 *
+	 * Note : not a blocking call
+	 */
+	void kill();
+
+	/**
+	 * The caller thread will wait until the thread has finished.
+	 *
+	 * Note : blocking call
+	 * @param killFirst if you want kill() to be called before joining (default false), otherwise not.
+	 */
+	void join(bool killFirst = false);
+
+    /**
+     * Set the thread priority.
+     * @param priority the priority
+     */
+    void setPriority(Priority priority);
+
+    /**
+	 * Set the thread affinity. This is applied during start of the thread.
+	 *
+	 * MAC OS X : http://developer.apple.com/library/mac/#releasenotes/Performance/RN-AffinityAPI/_index.html.
+	 * @param cpu the cpu id (start at 1), 0 means no affinity (default).
+	 */
+	void setAffinity(int cpu = 0);
+
+	/**
+	 * @return if the state of the thread is kSCreating (after start() is called but before entering the mainLoop()).
+	 */
+    bool isCreating() const;
+
+    /**
+	 * @return if the state of the thread is kSRunning (it is executing the mainLoop()) or kSCreating.
+	 */
+    bool isRunning() const;
+
+    /**
+	 * @return if the state of the thread is kSIdle (before start() is called and after the thread is totally killed (or after join(true))).
+	 */
+    bool isIdle() const;
+
+    /**
+	 * @return if the state of the thread is kSKilled (after kill() is called and before the thread is totally killed).
+	 */
+    bool isKilled() const;
+
+    Handle getThreadHandle() const {return handle_;}
+
+
+protected:
+
+private:
+	/**
+	 * Virtual method mainLoopBegin().
+	 * User can implement this function to add a behavior
+	 * before the main loop is started. It is
+	 * called once (before entering mainLoop()).
+	 */
+	virtual void mainLoopBegin() {}
+
+	/**
+	 * Pure virtual method mainLoop().
+	 * The inner loop of the thread. This method is called repetitively
+	 * until the thread is killed. Note that if kill() is called in mainLoopBegin(),
+	 * mainLoop() is not called, terminating immediately the thread.
+	 *
+	 * @see mainLoop()
+	 * @see kill()
+	 */
+	virtual void mainLoop() = 0;
+
+	/**
+	 * Virtual method mainLoopKill().
+	 * User can implement this function to add a behavior
+	 * before the thread is killed. When this
+	 * function is called, the state of the thread is set to kSKilled. It is useful to
+	 * wake up a sleeping thread to finish his loop and to avoid a deadlock.
+	 */
+	virtual void mainLoopKill() {}
+
+	/**
+	 * Virtual method mainLoopEnd().
+	 * User can implement this function to add a behavior
+	 * after the thread is killed (after exiting the mainLoop(), work is
+	 * still done in the thread before exiting).
+	 */
+	virtual void mainLoopEnd() {}
+
+    /*
+     * Inherited method ThreadMain() from Thread.
+     * @see Thread<void>
+     */
+    void ThreadMain();
+
+    /*
+	 * Apply thread priority. This is called when starting the thread.
+	 * *@todo : Support pthread
+	 */
+	void applyPriority();
+
+	/*
+	 * Apply cpu affinity. This is called when starting the thread.
+	 * *@todo : Support Windows
+	 */
+	void applyAffinity();
+
+    /*
+     * Inherited method Create() from Thread.
+     * Here we force this function to be private so the
+     * inherited class can't have access to it.
+     * @see Thread<void>
+     */
+    int Create(
+      Handle  * const     & H               = 0,
+      const bool          & CreateDetached  = false,
+      const unsigned int  & StackSize       = 0,
+      const bool          & CancelEnable    = false,   // UNUSED
+      const bool          & CancelAsync     = false    // UNUSED
+    ) const;
+
+    //Methods from UThread<void> class hided
+    static int Join( Handle H )
+	  { return UThreadC<void>::Join(H); }
+	static int Kill( Handle H )
+	  { return UThreadC<void>::Kill(H); }
+	static int Detach( Handle H )
+	  { return UThreadC<void>::Detach(H); }
+
+private:
+	void operator=(UThread & t) {}
+	UThread( const UThread & t ) {}
+
+private:
+    enum State{kSIdle, kSCreating, kSRunning, kSKilled}; /* Enum of states. */
+    State state_; 			/* The thread state. */
+    Priority priority_; 	/* The thread priority. */
+    Handle handle_; 	/* The thread handle. */
+#ifdef WIN32
+    int threadId_; 			/* The thread id. */
+#else
+    unsigned long threadId_; /* The thread id. */
+#endif
+    int cpuAffinity_; /* The cpu affinity. */
+    UMutex killSafelyMutex_;	/* Mutex used to protect the kill() method. */
+    UMutex runningMutex_;	    /* Mutex used to notify the join method when the thread has finished. */
+};
+
+#endif // UTHREADNODE_H

utilite/include/rtabmap/utilite/UThreadC.h

@@ -0,0 +1,36 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTHREADC_H
+#define UTHREADC_H
+
+#include <errno.h>
+
+/*
+ * Use of StateThread is safer. The Thread class in his
+ * base form is not supported.
+ * @see StateThread
+ */
+#ifdef WIN32
+  #include "rtabmap/utilite/Win32/UThreadC.h"
+#else
+  #include "rtabmap/utilite/Posix/UThreadC.h"
+#endif
+
+#endif // UTHREADC_H

utilite/include/rtabmap/utilite/UThreadNode.h

@@ -0,0 +1,29 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTHREADNODE_H_
+#define UTHREADNODE_H_
+
+#include "rtabmap/utilite/UThread.h"
+
+//For backward compatibility
+typedef UThread UThreadNode;
+
+
+#endif /* UTHREADNODE_H_ */

utilite/include/rtabmap/utilite/UTimer.h

@@ -0,0 +1,109 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTIMER_H
+#define UTIMER_H
+
+#include "UtiLiteExp.h" // DLL export/import defines
+
+#ifdef WIN32
+#include <windows.h>
+#else
+#include <sys/time.h>
+#include <time.h>
+#endif
+
+/**
+ * This class is used to time some codes (in seconds).
+ * On Unix, the resolution is up to microseconds (see gettimeofday()).
+ * On Windows, the performance counter is used (see QueryPerformanceCounter() and QueryPerformanceFrequency()).
+ * Example:
+ * @code
+ *      UTimer timer;
+ *      timer.start();
+ *      ... (do some work)
+ *      timer.stop();
+ *      double seconds = timer.getInterval();
+ *      ...
+ * @endcode
+ */
+class UTILITE_EXP UTimer
+{
+public:
+    UTimer();
+    ~UTimer();
+
+    /** 
+     * This method is used to get 
+     * the time of the system right now.
+     * @return double the time in seconds.
+     */
+    static double now();
+
+    /** 
+     * This method starts the timer.
+     */
+    void start();
+
+    /** 
+     * This method stops the timer.
+     */
+    void stop();
+
+    /** 
+     * This method is used to get the elapsed time
+     * between now and the start(). If timer is stopped, the interval time
+	 * between stop() and the start() is returned.
+     * @return double the interval in seconds.
+     */
+    double elapsed() {return getElapsedTime();}
+    double getElapsedTime();
+
+    /**
+	 * This method is used to get the interval time
+	 * between stop() and the start().
+	 * @return double the interval in seconds.
+	 * @deprecated use elapsed() instead.
+	 */
+    DEPRECATED(double getInterval());
+
+    /** 
+     * This method is used to get the interval of 
+     * the timer while it is running. It's automatically 
+     * stop the timer, get the interval and restart 
+     * the timer. It's the same of calling stop(), 
+     * elapsed() and start(). Method restart() does the same thing, for convenience.
+     * @return double the interval in seconds.
+     */
+    double restart() {return ticks();}
+    double ticks();
+
+private:
+#ifdef WIN32
+    LARGE_INTEGER startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
+    LARGE_INTEGER stopTimeRecorded_;  /* When we stop the timer. */
+
+    LARGE_INTEGER frequency_;          /* Keep the frequency of the counter */
+#else
+    struct timeval startTimeRecorded_; /* When we start the timer, timeRecorded is copied over lastTimeRecorded.*/
+    struct timeval stopTimeRecorded_;  /* When we stop the timer. */
+#endif
+};
+
+#endif //UTIMER_H

utilite/include/rtabmap/utilite/UVariant.h

@@ -0,0 +1,66 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UVARIANT_H
+#define UVARIANT_H
+
+#include <string>
+
+/**
+ * Experimental class...
+ */
+class UVariant
+{
+public:
+	virtual ~UVariant() {}
+
+	virtual std::string className() const = 0;
+
+	template<class T>
+	const T * data() const {
+		if(data_)
+			return (T*)data_;
+		return (const T*)constData_;
+	}
+
+	template<class T>
+	T * takeDataOwnership() {
+		T * data = (T*)data_;
+		constData_ = 0;
+		data_=0;
+		return data;
+	}
+
+protected:
+	UVariant(void * data) :
+		data_(data),
+		constData_(0)
+	{}
+
+	UVariant(const void * data) :
+		data_(0),
+		constData_(data)
+	{}
+
+protected:
+	void * data_;
+	const void * constData_;
+};
+
+#endif /* UVARIANT_H */

utilite/include/rtabmap/utilite/UtiLite.h

@@ -0,0 +1,326 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTILITE_H
+#define UTILITE_H
+
+/** \mainpage UtiLite
+  *
+  * \section intro Introduction
+  * <a href="http://utilite.googlecode.com">UtiLite</a> is a simple library to create small cross-platform
+  * applications using <b>threads</b>, <b>events-based communication</b> and a powerful <b>logger</b>. The first three
+  * sections show the core classes of the library, then last sections show some useful functions added through time.
+  *
+  * UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
+  * @code
+  * $ ./uresourcegenerator DatabaseSchema.sql
+  * @endcode
+  * This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
+  * Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
+  * @code
+  * #include "DatabaseSchema_sql.h"
+  * ...
+  * std::string hex = DATABASESCHEMA_SQL;
+  * // Assuming there are only ASCII characters, we can directly convert to a string:
+  * std::string schema = uHex2Str(hex);
+  * // For binary data:
+  * std::vector<char> bytes = uHex2Bytes(hex);
+  * @endcode
+  *
+  * A generated \ref findUtilitePage "FindUtiLite.cmake" is also provided for easy linking with the library.
+  *
+  *
+  * \section logger ULogger
+  * The ULogger can be used anywhere in the application to log messages (formated like a printf). The
+  * logger can be set (ULogger::setType()) to print in a file or in the console (with colors depending on the severity). Convenient
+  * macros are given, working like a printf(), as UDEBUG(), UINFO(), UWARN(), UERROR(), UFATAL(). Small example:
+  * @code
+  * ...
+  * UINFO("A simple message with number %d", 42);
+  * UDEBUG("A debug message");
+  * ...
+  * @endcode
+  * This will print: [Severity] (Time) File:Line:Function() "The message"
+  * @code
+  * [ INFO] (2010-09-25 18:08:20) main.cpp:18::main() A simple message with number 42
+  * [DEBUG] (2010-09-25 18:08:20) main.cpp:18::main() A debug message
+  * @endcode
+  *
+  * \section eventsmanager UEventsManager, UEventsHandler, UEvent
+  * The events-based communication framework helps to communicate between objects/threads.
+  * The UEventsManager is a singleton with which we can post events anywhere in the
+  * application by calling UEventsManager::post(). All UEventsHandler will then receive the
+  * event with their protected function UEventsHandler::handleEvent(). Handlers are added to UEventsManager by
+  * calling UEventsManager::addHandler(). The UEvent provides an abstract class to implement any event
+  * implementations. The only requirement is that the event must implements UEvent::getClassName() to know the event's type.
+  * @code
+  * ...
+  * MyHandler handler; // MyHandler is an implementation of UEventsHandler
+  * UEventsManager::addHandler(&handler);
+  * UEventsManager::post(new MyEvent()); // MyEvent is an implementation of UEvent
+  * // The UEventsHandler::handleEvent() of "handler" will then be called by the UEventsManager's events dispatching thread.
+  * ...
+  * @endcode
+  * Look at the <b>full example</b> in page of UEventsHandler on how communication works with threads (UThreadNode).
+  *
+  * \section threadnode UThreadNode, UMutex, USemaphore
+  * The multi-threading framework use a UThreadNode as an abstract class to implement a
+  * thread in object-style. Reimplement UThreadNode::mainLoop() then call UThreadNode::start() to
+  * start the main loop of the thread. Threads can be joined by calling UThreadNode::join() and
+  * killed by calling UThreadNode::kill(). Classes UMutex and USemaphore provide blocking mechanisms to
+  * protect data between threads.
+  * @code
+  * ...
+  * MyThread t; // MyThread is an implementation of UThreadNode
+  * t.start();
+  * t.join(); // Wait the thread to finish
+  * ...
+  * @endcode
+  *
+  * \section timer UTimer
+  * A useful class to compute processing time:
+  *  - UTimer::start(),
+  *  - UTimer::stop(),
+  *  - UTimer::restart(),
+  *  - UTimer::elapsed(),
+  *  - UTimer::now().
+  *
+  * \section filedir UDirectory, UFile
+  * For files and directories manipulations :
+  *  - UFile::exists(),
+  *  - UFile::length(),
+  *  - UFile::rename(),
+  *  - UFile::erase(),
+  *  - UDirectory::exists(),
+  *  - UDirectory::getFileNames(),
+  *  - UDirectory::makeDir(),
+  *  - UDirectory::removeDir(),
+  *  - UDirectory::currentDir(),
+  *  - UDirectory::homeDir(),
+  *
+  * \section stl Convenient use of STL
+  * The library provides some simple wrappers of the STL like:
+  *  - uUniqueKeys() to get unique keys from a std::multimap,
+  *  - uKeys() to get all keys of a std::multimap or std::map,
+  *  - uValues() to get all values of a std::multimap or std::map,
+  *  - uValue() to get the value of a key (with a default argument if the key is not found),
+  *  - uTake() to take the value of a key (with a default argument if the key is not found),
+  *  - uIteratorAt() to get iterator at a specified position in a std::list,
+  *  - uValueAt() to get value at a specified position in a std::list,
+  *  - uContains() to know if a key/value exists in a std::multimap, std::map, std::list,
+  *  - uInsert() to insert a value in a std::map and overwriting the value if the key already exists,
+  *  - uListToVector(),
+  *  - uVectorToList(),
+  *  - uAppend() to append a list to another list,
+  *  - uIndexOf() to get index of a value in a std::list,
+  *  - uSplit() to split a string into a std::list of strings on the specified separator.
+  *
+  *
+  * \section math Basic mathematic operations
+  * A library of basic array manipulations:
+  *  - uMax(),
+  *  - uSign(),
+  *  - uSum(),
+  *  - uMean(),
+  *  - uStdDev(),
+  *  - uNorm(),
+  *  - uNormalize(),
+  *  - uXMatch().
+  *
+  * \section conversion Conversion
+  * A library of convenient functions to convert some data into another like:
+  * - uReplaceChar(),
+  * - uToUpperCase(),
+  * - uToLowerCase(),
+  * - uNumber2Str(),
+  * - uBool2Str(),
+  * - uStr2Bool(),
+  * - uBytes2Hex(),
+  * - uHex2Bytes(),
+  * - uHex2Bytes(),
+  * - uHex2Str(),
+  * - uHex2Ascii(),
+  * - uAscii2Hex(),
+  * - uFormatv(),
+  * - uFormat().
+  *
+  * \section processinfo UProcessInfo
+  * This class can be used to get the process memory usage: UProcessInfo::getMemoryUsage().
+  *
+  * \section qtLib Qt Widgets (libutilite_qt.so : OPTIONAL)
+  * If Qt is found on the system, the UtiLite Qt library (libutilite_qt.so, libutilite_qt.dll) with
+  * useful widgets is built. Use class UPlot to create a plot like MATLAB, and incrementally add
+  * new values like a scope. USpectrogram is used to
+  * show audio frequency frames.
+  * @image html UPlot.gif
+  * @image html USpectrogram.png
+  *
+  * \section audioLib Audio stuff (libutilite_audio.so : OPTIONAL)
+  * If FMOD is found on the system, the UtiLite audio
+  * library is built (libutilite_audio.so, libutilite_audio.dll). It is a wrapper
+  * of FMOD methods with a convenient interface to extract audio frames.
+  * - UAudioRecorder,
+  * - UAudioRecorderFile,
+  * - UAudioRecorderMic
+  * - UAudioPlayer,
+  * - UAudioPlayerTone,
+  * - UWav,
+  * - UMp3Encoder (only if Lame is also found on the system).
+  */
+
+/*! \page uResourceGeneratorPage uResourceGenerator
+ * UtiLite provides a utility application called \ref uResourceGeneratorPage "uResourceGenerator" to generate resources to include in an executable. For example:
+ * @code
+ * $ ./uresourcegenerator DatabaseSchema.sql
+ * @endcode
+ * This will generate a HEX file "DatabaseSchema_sql.h" which can be included in source files.
+ * Data of the file is global and can be accessed by the generated const char * DATABASESCHEMA_SQL.
+ * @code
+ * #include "DatabaseSchema_sql.h"
+ * ...
+ * std::string hex = DATABASESCHEMA_SQL;
+ * // Assuming there are only ASCII characters, we can directly convert to a string:
+ * std::string schema = uHex2Str(hex);
+ * // For binary data:
+ * std::vector<char> bytes = uHex2Bytes(hex);
+ * @endcode
+ *
+ * The generator can be automated in a CMake build like:
+ * @code
+ * ADD_CUSTOM_COMMAND(
+ *    OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
+ *    COMMAND ${URESOURCEGENERATOR_EXEC} -n my_namespace -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
+ *    COMMENT "[Creating database resource]"
+ *    DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/DatabaseSchema.sql
+ * )
+ * SET(RESOURCES
+ *    ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
+ * )
+ * ADD_LIBRARY(mylib ${SRC_FILES} ${RESOURCES})
+ * ADD_EXECUTABLE(myexecutable ${SRC_FILES} ${RESOURCES})
+ * @endcode
+ * The variable URESOURCEGENERATOR_EXEC is set when FIND_PACKAGE(UtiLite) is done, you would need to add \ref findUtilitePage "FindUtiLite.cmake".
+ */
+
+/*! \page findUtilitePage FindUtilite.cmake
+ * UtiLite provides a generated \ref findUtilitePage "FindUtiLite.cmake" for easy linking with the library. Here
+ * is an example but you should take the one in the build folder of the library.
+ * @code
+# - Find UTILITE
+# This module finds an installed UTILITE package.
+#
+# It sets the following variables:
+#  UTILITE_FOUND              - Set to false, or undefined, if UTILITE isn't found.
+#  UTILITE_INCLUDE_DIRS        - The UTILITE include directory.
+#  UTILITE_LIBRARIES            - The UTILITE library to link against.
+#  URESOURCEGENERATOR_EXEC    - The resource generator tool executable
+#
+#
+
+SET(UTILITE_VERSION_REQUIRED 0.2.13)
+
+SET(UTILITE_ROOT)
+
+# Add ROS UtiLite directory if ROS is installed
+FIND_PROGRAM(ROSPACK_EXEC NAME rospack PATHS)
+IF(ROSPACK_EXEC)
+	EXECUTE_PROCESS(COMMAND ${ROSPACK_EXEC} find utilite
+			   	    OUTPUT_VARIABLE UTILITE_ROS_PATH
+					OUTPUT_STRIP_TRAILING_WHITESPACE
+					WORKING_DIRECTORY "./"
+	)
+	IF(UTILITE_ROS_PATH)
+	    MESSAGE(STATUS "Found UtiLite ROS pkg : ${UTILITE_ROS_PATH}")
+	    SET(UTILITE_ROOT
+	        ${UTILITE_ROS_PATH}/utilite
+	        ${UTILITE_ROOT}
+	    )
+	ENDIF(UTILITE_ROS_PATH)
+ENDIF(ROSPACK_EXEC)
+
+FIND_PROGRAM(URESOURCEGENERATOR_EXEC NAME uresourcegenerator PATHS ${UTILITE_ROOT}/bin)
+IF(URESOURCEGENERATOR_EXEC)
+	EXECUTE_PROCESS(COMMAND ${URESOURCEGENERATOR_EXEC} -v
+			   	    OUTPUT_VARIABLE UTILITE_VERSION
+					OUTPUT_STRIP_TRAILING_WHITESPACE
+					WORKING_DIRECTORY "./"
+	)
+
+	IF(UTILITE_VERSION VERSION_LESS UTILITE_VERSION_REQUIRED)
+	    IF(UtiLite_FIND_REQUIRED)
+	    	MESSAGE(FATAL_ERROR "Your version of UtiLite is too old (${UTILITE_VERSION}), UtiLite ${UTILITE_VERSION_REQUIRED} is required.")
+	    ENDIF(UtiLite_FIND_REQUIRED)
+	ENDIF(UTILITE_VERSION VERSION_LESS UTILITE_VERSION_REQUIRED)
+
+	IF(WIN32)
+		FIND_PATH(UTILITE_INCLUDE_DIRS
+				UEventsManager.h
+				PATH_SUFFIXES "../include")
+
+		FIND_LIBRARY(UTILITE_LIBRARIES NAMES utilite
+			 	PATH_SUFFIXES "../lib")
+
+	ELSE()
+		FIND_PATH(UTILITE_INCLUDE_DIRS
+				UEventsManager.h
+				PATHS ${UTILITE_ROOT}/include)
+
+		FIND_LIBRARY(UTILITE_LIBRARIES
+				NAMES utilite
+				PATHS ${UTILITE_ROOT}/lib)
+	ENDIF()
+
+	IF (UTILITE_INCLUDE_DIRS AND UTILITE_LIBRARIES)
+	   SET(UTILITE_FOUND TRUE)
+	ENDIF (UTILITE_INCLUDE_DIRS AND UTILITE_LIBRARIES)
+ENDIF(URESOURCEGENERATOR_EXEC)
+
+IF (UTILITE_FOUND)
+   # show which UTILITE was found only if not quiet
+   IF (NOT UtiLite_FIND_QUIETLY)
+      MESSAGE(STATUS "Found UtiLite ${UTILITE_VERSION}")
+   ENDIF (NOT UtiLite_FIND_QUIETLY)
+ELSE ()
+   # fatal error if UTILITE is required but not found
+   IF (UtiLite_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find UtiLite. Verify your PATH if it is already installed or download it at http://utilite.googlecode.com")
+   ENDIF (UtiLite_FIND_REQUIRED)
+ENDIF ()
+
+ * @endcode
+ *
+ */
+
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/utilite/UThreadNode.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UVariant.h"
+#include "rtabmap/utilite/UMath.h"
+
+#endif /* UTILITE_H */

utilite/include/rtabmap/utilite/UtiLiteExp.h

@@ -0,0 +1,47 @@
+/*
+*  utilite is a cross-platform library with
+*  useful utilities for fast and small developing.
+*  Copyright (C) 2010  Mathieu Labbe
+*
+*  utilite is free library: you can redistribute it and/or modify
+*  it under the terms of the GNU Lesser General Public License as published by
+*  the Free Software Foundation, either version 3 of the License, or
+*  (at your option) any later version.
+*
+*  utilite 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 Lesser General Public License for more details.
+*
+*  You should have received a copy of the GNU Lesser General Public License
+*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef UTILITEEXP_H
+#define UTILITEEXP_H
+
+/**
+ * Used mainly on Windows for dynamic linked libraries (dll).
+ */
+#if defined(_WIN32)
+  #if defined(utilite_EXPORTS) || defined(utilite_qt_EXPORTS) || defined(utilite_audio_EXPORTS) || defined(utilite_cv_EXPORTS) || defined(utilite_cvqt_EXPORTS)
+    #define UTILITE_EXP   __declspec( dllexport )
+  #else
+    #define UTILITE_EXP   __declspec( dllimport )
+  #endif
+#else
+  #define UTILITE_EXP
+#endif
+
+#ifdef __GNUC__
+#define DEPRECATED(func) func __attribute__ ((deprecated))
+#elif defined(_MSC_VER)
+#define DEPRECATED(func) __declspec(deprecated) func
+#else
+#pragma message("WARNING: You need to implement DEPRECATED for this compiler")
+#define DEPRECATED(func) func
+#endif
+
+#endif
+
+