Updated default Graph Optimization parameters (iteration+epsilon). Added "Variance from Inliers" parameter to DatabaseViewer

Merging devel to master

CMakeLists.txt

@@ -1,13 +1,17 @@
 # Top-Level CmakeLists.txt
-IF(APPLE OR WIN32)
-  # BundleUtilities.cmake from 2.8.3 required on Mac and ?Windows?
-  cmake_minimum_required(VERSION 2.8.3)
-ELSE()
-  cmake_minimum_required(VERSION 2.8.0)
-ENDIF()
+# For GNUInstallDirs
+cmake_minimum_required(VERSION 2.8.5)
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)
 
+# Catkin doesn't support multiarch library path, 
+# fix to "lib" if not set by user.
+IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+   set(CMAKE_INSTALL_LIBDIR "lib")
+ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+
+INCLUDE(GNUInstallDirs)
+
 ####### local cmake modules #######
 SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 
@@ -15,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 7)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_MINOR_VERSION 10)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -28,6 +32,8 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})
 
 SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 
+SET(RTABMAP_QT_VERSION 4 CACHE STRING "Which QT version to use")
+
 ####### COMPILATION PARAMS #######
 # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release:
 IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
@@ -82,44 +88,29 @@ IF(APPLE)
 ENDIF()
 
 ####### Build libraries as shared or static #######
-# It is a bit messed up on Windows between STATIC and SHARED, if we don't want
-# to add UtiLite_DEFINITIONS in FindUtiLite.cmake... to know from 3rd party application if it needs to
-# link dynamically [UTILITE_EXP __declspec( dllimport )] or statically (UTILITE_EXP "nothing").
 OPTION( BUILD_SHARED_LIBS "Set to OFF to build static libraries" ON )
 
-####### SET RPATH #########
-# When RPATH is activated (supported on most UNIX systems), 
-# the user doesn't need to change LD_LIBRARY_PATH
-
-# use, i.e. don't skip the full RPATH for the build tree
-SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
-
-# when building, don't use the install RPATH already
-# (but later on when installing)
-SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 
-
-# the RPATH to be used when installing
-SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}")
-
-# add the automatically determined parts of the RPATH
-# which point to directories outside the build tree to the install RPATH
-SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 ####### OUTPUT DIR #######
 SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
 
+# Avoid Visual Studio bin/Release and bin/Debug sub directories
+SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
+SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
+SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}")
+SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}")
+SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+
 ####### INSTALL DIR #######
-# Offer the user the choice of overriding the installation directories
-set(INSTALL_LIB_DIR lib/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH "Installation directory for libraries")
-set(INSTALL_BIN_DIR bin CACHE PATH "Installation directory for executables")
 set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
   "Installation directory for header files")
 if(WIN32 AND NOT CYGWIN)
   set(DEF_INSTALL_CMAKE_DIR CMake)
 else()
-  set(DEF_INSTALL_CMAKE_DIR lib/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
+  set(DEF_INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 endif()
 set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
   "Installation directory for CMake files")
@@ -130,28 +121,101 @@ IF(APPLE)
 ENDIF(APPLE)
 
 ####### DEPENDENCIES #######
-FIND_PACKAGE(OpenCV REQUIRED)
-IF(OPENCV_NONFREE_FOUND)
-  ADD_DEFINITIONS(-DWITH_NONFREE)
-  SET(NONFREE 1)
-ELSE()
-  SET(NONFREE 0)
-ENDIF()
-FIND_PACKAGE(PCL 1.7 REQUIRED)
-FIND_PACKAGE(VTK REQUIRED)
-IF("${VTK_MAJOR_VERSION}" EQUAL 5)
-	FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
-ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
-FIND_PACKAGE(ZLIB REQUIRED)
-FIND_PACKAGE(Freenect)
-FIND_PACKAGE(OpenNI2)
 
+option(WITH_QT            "Include Qt support"                   ON)
+option(WITH_FREENECT      "Include Freenect support"             ON)
+option(WITH_FREENECT2     "Include Freenect2 support"            ON)
+option(WITH_OPENNI2       "Include OpenNI2 support"              ON)
+option(WITH_DC1394        "Include dc1394 support"               ON)
+option(WITH_G2O           "Include g2o support"                  ON)
+option(WITH_GTSAM         "Include GTSAM support"                ON)
+option(WITH_CVSBA         "Include cvsba support"                ON)
+option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
+
+FIND_PACKAGE(OpenCV REQUIRED QUIET)
+FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
+FIND_PACKAGE(ZLIB REQUIRED QUIET)
+IF(OpenCV_FOUND)
+   MESSAGE(STATUS "Found OpenCV: ${OpenCV_INCLUDE_DIRS}")
+ENDIF(OpenCV_FOUND)
+IF(PCL_FOUND)
+   MESSAGE(STATUS "Found PCL: ${PCL_INCLUDE_DIRS}")
+ENDIF(PCL_FOUND)
+IF(ZLIB_FOUND)
+   MESSAGE(STATUS "Found ZLIB: ${ZLIB_INCLUDE_DIRS}")
+ENDIF(ZLIB_FOUND)
+
+IF(WITH_QT)
 # If Qt is here, the GUI will be built
-FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) 
+    IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+    ELSE()
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+    ENDIF()
+    IF(QT4_FOUND OR Qt5_FOUND)
+        FIND_PACKAGE(VTK REQUIRED)
+        IF("${VTK_MAJOR_VERSION}" EQUAL 5)
+            FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
+        ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
+    ENDIF(QT4_FOUND OR Qt5_FOUND)
+ENDIF(WITH_QT)
+
+IF(WITH_FREENECT)
+    FIND_PACKAGE(Freenect QUIET)
+    IF(Freenect_FOUND)
+       MESSAGE(STATUS "Found Freenect: ${Freenect_INCLUDE_DIRS}")
+    ENDIF(Freenect_FOUND)
+ENDIF(WITH_FREENECT)
+
+IF(WITH_FREENECT2)
+    FIND_PACKAGE(freenect2 QUIET)
+    IF(freenect2_FOUND)
+       MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}")
+    ENDIF(freenect2_FOUND)
+ENDIF(WITH_FREENECT2)
+
+IF(WITH_OPENNI2)
+    FIND_PACKAGE(OpenNI2 QUIET)
+    IF(OpenNI2_FOUND)
+       MESSAGE(STATUS "Found OpenNI2: ${OpenNI2_INCLUDE_DIRS}")
+    ENDIF(OpenNI2_FOUND)
+ENDIF(WITH_OPENNI2)
+
+IF(WITH_DC1394)
+    FIND_PACKAGE(DC1394 QUIET)
+    IF(DC1394_FOUND)
+       MESSAGE(STATUS "Found DC1394: ${DC1394_INCLUDE_DIRS}")
+    ENDIF(DC1394_FOUND)
+ENDIF(WITH_DC1394)
+
+IF(WITH_G2O)
+    FIND_PACKAGE(G2O QUIET)
+    IF(G2O_FOUND)
+       MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+    ENDIF(G2O_FOUND)
+ENDIF(WITH_G2O)
+
+IF(WITH_GTSAM)
+    FIND_PACKAGE(GTSAM QUIET)
+ENDIF(WITH_GTSAM)
+
+IF(WITH_FLYCAPTURE2)
+    FIND_PACKAGE(FlyCapture2 QUIET)
+    IF(FlyCapture2_FOUND)
+       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
+    ENDIF(FlyCapture2_FOUND)
+ENDIF(WITH_FLYCAPTURE2)
+
+IF(WITH_CVSBA)
+    FIND_PACKAGE(cvsba)
+    IF(cvsba_FOUND)
+       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
+    ENDIF(cvsba_FOUND)
+ENDIF(WITH_CVSBA)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
-  IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+  IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
        
     # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/".
     SET(CMAKE_INSTALL_PREFIX "/")
@@ -178,16 +242,20 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 
 ####### SOURCES (Projects) #######
+SET(NONFREE 0)
+IF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
+   SET(NONFREE 1)
+ENDIF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
 
-IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
    ADD_SUBDIRECTORY( guilib )
    ADD_SUBDIRECTORY( app )
-ELSE()
-   MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI lib and the stand-alone application will not be compiled...")
+ELSEIF(WITH_QT)
+   MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...")
 ENDIF()
 
 ADD_SUBDIRECTORY( tools )
@@ -209,13 +277,14 @@ ADD_CUSTOM_TARGET(uninstall
 ####
 # Create the RTABMapConfig.cmake and RTABMapConfigVersion files
 file(RELATIVE_PATH REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_INCLUDE_DIR}")
-file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${INSTALL_LIB_DIR}")
+file(RELATIVE_PATH REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
    
 # ... for the build tree
 set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" 
 					  "${PROJECT_SOURCE_DIR}/guilib/include" 
 					  "${PROJECT_SOURCE_DIR}/utilite/include")
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+set(CONF_WITH_GUI ${WITH_QT})
 configure_file(RTABMapConfig.cmake.in
   "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY)
   
@@ -237,7 +306,7 @@ install(FILES
 ####
 
 ### Install package.xml for catkin
-install(FILES package.xml DESTINATION "share/${PROJECT_PREFIX}")
+install(FILES package.xml DESTINATION "${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_PREFIX}")
 
 #######################
 # CPACK (Packaging)
@@ -279,10 +348,11 @@ IF(WIN32)
   ENDIF()
   SET(CPACK_GENERATOR "ZIP;NSIS")
   SET(CPACK_SOURCE_GENERATOR "ZIP")
-  SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME} ${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}")
+  SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
   SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
   SET(CPACK_NSIS_MUI_ICON ${ICON_PATH})
   SET(CPACK_NSIS_MUI_UNIICON ${ICON_PATH})
+  SET(CPACK_PACKAGE_INSTALL_DIRECTORY "${PROJECT_NAME}")
   #SET(CPACK_PACKAGE_ICON ${ICON_PATH})
   #SET(CPACK_NSIS_INSTALLED_ICON_NAME ${ICON_PATH})
   #SET(CPACK_NSIS_HELP_LINK "http:\\\\\\\\www.${PROJECT_PREFIX}.googlecode.com")
@@ -330,22 +400,94 @@ IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
 
-IF(OPENCV_NONFREE_FOUND)
-MESSAGE(STATUS "  With OpenCV nonfree module (SIFT/SURF) = YES")
-ELSE()
-MESSAGE(STATUS "  With OpenCV nonfree module (SIFT/SURF) = NO (not found)")
-ENDIF()
+IF(OpenCV_FOUND)
+  IF(OpenCV_VERSION_MAJOR EQUAL 2)
+    IF(OPENCV_NONFREE_FOUND)
+      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES")
+    ELSE()
+      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found)")
+    ENDIF()
+  ELSE()
+    IF(OPENCV_XFEATURES2D_FOUND)
+      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES")
+    ELSE()
+      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found)")
+    ENDIF()
+  ENDIF()
+ENDIF(OpenCV_FOUND)
 
 IF(Freenect_FOUND)
-MESSAGE(STATUS "  With Freenect = YES")
+MESSAGE(STATUS "  With Freenect             = YES")
+ELSEIF(NOT WITH_FREENECT)
+MESSAGE(STATUS "  With Freenect             = NO (WITH_FREENECT=OFF)")
 ELSE()
-MESSAGE(STATUS "  With Freenect = NO (libfreenect not found)")
+MESSAGE(STATUS "  With Freenect             = NO (libfreenect not found)")
 ENDIF()
 
 IF(OpenNI2_FOUND)
-MESSAGE(STATUS "  With OpenNI2  = YES")
+MESSAGE(STATUS "  With OpenNI2              = YES")
+ELSEIF(NOT WITH_OPENNI2)
+MESSAGE(STATUS "  With OpenNI2              = NO (WITH_OPENNI2=OFF)")
 ELSE()
-MESSAGE(STATUS "  With OpenNI2  = NO (OpenNI2 not found)")
+MESSAGE(STATUS "  With OpenNI2              = NO (OpenNI2 not found)")
+ENDIF()
+
+IF(freenect2_FOUND)
+MESSAGE(STATUS "  With Freenect2            = YES")
+ELSEIF(NOT WITH_FREENECT2)
+MESSAGE(STATUS "  With Freenect2            = NO (WITH_FREENECT2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
+ENDIF()
+
+IF(DC1394_FOUND)
+MESSAGE(STATUS "  With dc1394               = YES")
+ELSEIF(NOT WITH_DC1394)
+MESSAGE(STATUS "  With dc1394               = NO (WITH_DC1394=OFF)")
+ELSE()
+MESSAGE(STATUS "  With dc1394               = NO (dc1394 not found)")
+ENDIF()
+
+IF(FlyCapture2_FOUND)
+MESSAGE(STATUS "  With FlyCapture2/Triclops = YES")
+ELSEIF(NOT WITH_FLYCAPTURE2)
+MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (WITH_FLYCAPTURE2=OFF)")
+ELSE()
+MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
+ENDIF()
+
+IF(G2O_FOUND)
+MESSAGE(STATUS "  With g2o                  = YES")
+ELSEIF(NOT WITH_G2O)
+MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
+ELSE()
+MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
+ENDIF()
+
+IF(GTSAM_FOUND)
+MESSAGE(STATUS "  With GTSAM                = YES")
+ELSEIF(NOT WITH_GTSAM)
+MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+ENDIF()
+
+IF(cvsba_FOUND)
+MESSAGE(STATUS "  With cvsba                = YES")
+ELSEIF(NOT WITH_CVSBA)
+MESSAGE(STATUS "  With cvsba                = NO (WITH_CVSBA=OFF)")
+ELSE()
+MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
+ENDIF()
+
+IF(QT4_FOUND)
+MESSAGE(STATUS "  With Qt                   = YES (version 4)")
+ELSEIF(Qt5_FOUND)
+MESSAGE(STATUS "  With Qt                   = YES (version 5)")
+ELSEIF(NOT WITH_QT)
+MESSAGE(STATUS "  With Qt                   = NO (WITH_QT=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Qt                   = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
 ENDIF()
 
 MESSAGE(STATUS "--------------------------------------------")

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

Matlab/ParticleFilter/pf_filter.m

@@ -0,0 +1,24 @@
+
+function filtered = pf_filter(x, nParticles, noise, lambda)
+
+particles = ones(nParticles,1)*x(1) ;
+weights = ones(nParticles,1);
+filtered=zeros(1,length(x));
+for i = 1:length(x);
+    for j = 1:nParticles 
+        rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++
+        noisyP = particles(j) + noise*rn ;
+        dist = abs(noisyP - x(i));
+        tmp = exp(-lambda*dist);
+        if isfinite(tmp) && tmp > 0
+            particles(j) = noisyP;
+            weights(j) = tmp;
+        end
+    end
+    if sum(weights(:)) > 0
+         weights = weights ./sum(weights(:));
+    end
+   
+    filtered(i) = weights'*particles;
+    particles = pf_resample(particles, weights);
+end

Matlab/ParticleFilter/pf_filter.m~

@@ -0,0 +1,18 @@
+
+function filtered = pf_filter(x, nParticles, noise, lambda)
+
+particles = zeros(nParticles,1) ;
+weights = zeros(nParticles,1);
+filtered=zeros(1,length(x));
+for i = 1:length(x);
+    for j = 1:nParticles 
+        rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++
+        bruit= noise*rn;
+        particles(j) = particles(j) + bruit ;
+        dist = abs(particles(j) - x(i));
+        weights(j) = exp(-lambda*dist);
+    end
+    weights = weights ./(sum(weights(:)));
+    filtered(i) = weights'*particles;
+    particles = Rresample2(particles,weights);
+end

Matlab/ParticleFilter/pf_resample.m

@@ -0,0 +1,23 @@
+
+function newParticles=pf_resample(particles,weights)
+pcum = zeros(length(weights),1);
+sum = 0;
+for i=1:length(weights)
+    pcum(i) = weights(i) + sum;
+    sum = sum + weights(i);
+end
+pcum = pcum./pcum(end);
+newParticles = 0.*particles;
+
+%
+for i = 1:length(newParticles)
+    indexx = 1;
+    randnum = rand;
+    for j = 1:length(pcum)
+        if(randnum < pcum(j))
+            indexx = j;
+            break;
+        end
+    end
+    newParticles(i) = particles(indexx);
+end

Matlab/ParticleFilter/test_kinect.m

@@ -0,0 +1,67 @@
+
+%close all
+
+% signals
+index = [1 2 4 5 6 8 10 12 13 14 15 17 18 20 21 23 24 25 26 28 29 31 32 33 35 36 38 39 41 42 43 45 46 48 49 50 51 52 53 55 56 58 59 60 62 63 65 66 68 70 72 73 74 75 77 78 80 81 83 84 86 88 90 91 92 94 95 96 98 100 101 103 105 106 108 109 111 113 114 116 117 118 120 122 123 125 126 128 129 131 132 134 135 137 138 139 141 142 144 145 146 148 149 150 152 153 154 156 157 159 161 162 164 165 167 168 169 171 172 174 176 177 178 180 181 182 184 186 187 188 189 191 193 195 196 198 199 201 202 203 205 206 207 208 210 212 213 216 217 218 220 221 223 224 225 227 228 229 230 232 233 234 236 237 239 240 241 243 244 246 247 249 250 251 253 254 256 257 259 260 262 263 264 265 266 268 269 270 273 274 276 277 280 281 283 284 286 288 289 291 293 294 296 297 299 301 302 303 304 305 307 308 310 311 313 314 316 317 318 320 322 323 325 326 328 329 330 331 333 335 338 339 340 342 343 345 347 348 350 352 354 355 357 359 361 363 365 368 369 370 372 375 378 380 383 386 389 390 392 394 396 398 401 404 407 410 413 415 418 421 423 425 428 431 434 437 440 443 446 449 452 455 459 462 464 467 469 472 475 478 481 484 487 490 493 496 499 501 503 506 509 512 514 517 520 521 523 526 529 531 534 536 538 540];
+
+stddev = [0 0.00183007 0.00192045 0.00161173 0.00109756 0.0016129 0.00187094 0.00164845 0.00172004 0.00178055 0.00146903 0.00153716 0.00153812 0.00185564 0.00165944 0.00178402 0.00177258 0.0110605 0.0186308 0.00726018 0.0096298 0.00578044 0.0173129 0.010495 0.00641252 0.0140946 0.00828691 0.00646527 0.0134895 0.00693482 0.00649181 0.0181309 0.0131438 0.00996371 0.00707931 0.0103485 0.0061651 0.00802035 0.0132984 0.00562768 0.00741177 0.0116417 0.00769641 0.00804565 0.05021 0.00682934 0.0129143 0.0225555 0.0127159 0.01415 0.0380939 0.0259584 0.0158027 0.0211238 0.0110875 0.0276258 0.0280592 0.0166966 0.0130543 0.0215521 0.0142676 0.0153512 0.0316784 0.0118649 0.0123691 0.0205413 0.0135362 0.0216125 0.0212237 0.00991641 0.0184909 0.0261524 0.0119713 0.0201614 0.0124039 0.0147738 0.0264302 0.0159957 0.0253929 0.0102058 0.0243234 0.0377172 0.0165959 0.0337177 0.0311854 0.0129289 0.0306891 0.0156638 0.0129385 0.0346115 0.0108297 0.0267145 0.0143579 0.0151814 0.0120711 0.0234515 0.010673 0.0141592 0.0133022 0.0140912 0.0109111 0.00720432 0.00984503 0.00544388 0.0150391 0.0120823 0.00699634 0.00620808 0.00564909 0.00469504 0.00484 0.0103237 0.00416761 0.00430465 0.00704729 0.004031 0.00422873 0.00754686 0.00478419 0.00442305 0.00741142 0.00629216 0.00676839 0.0068492 0.00492891 0.00640475 0.00507572 0.010178 0.0131225 0.00749722 0.00502731 0.00653215 0.00653063 0.00557653 0.00488872 0.00889771 0.0062636 0.00854236 0.00660393 0.00829196 0.00756908 0.00466529 0.00435093 0.00407168 0.00518314 0.00739503 0.0108294 0.00535951 0.00556133 0.00516625 0.0107237 0.00540061 0.0066455 0.00579536 0.00673659 0.00604048 0.00707398 0.0170932 0.00686887 0.0070607 0.00701904 0.0063693 0.00857325 0.00773534 0.0148109 0.0138909 0.013436 0.00893684 0.0087548 0.0108629 0.023048 0.011821 0.0163904 0.00790121 0.0069128 0.0110736 0.0111562 0.00968563 0.00775927 0.00795869 0.0080748 0.00909579 0.011114 0.00957061 0.0114517 0.011365 0.0113641 0.012989 0.0115229 0.012728 0.0104824 0.012118 0.0156755 0.0312968 0.0221914 0.0130828 0.0245588 0.00755494 0.00518046 0.00578518 0.0165867 0.0193008 0.0113112 0.0081156 0.00917008 0.00480015 0.0041285 0.0042448 0.00499233 0.00531906 0.00434526 0.00711454 0.00767021 0.00522772 0.00435821 0.00478461 0.00454364 0.00498411 0.00459049 0.00635743 0.00710944 0.00546137 0.00624892 0.0101038 0.00895114 0.00736796 0.00727758 0.00951425 0.0115899 0.00858932 0.0374993 0.0078321 0.00838064 0.0191884 0.0116717 0.0297484 0.0114346 0.0109484 0.02485 0.0117224 0.0167142 0.0107344 0.0188225 0.0123141 0.0273968 0.014611 0.0341862 0.0134783 0.0271164 0.0268258 0.0128071 0.0106398 0.0125586 0.0319245 0.0107098 0.0147609 0.0120215 0.0106572 0.0162843 0.0153122 0.010042 0.011171 0.0121647 0.0102679 0.00730296 0.0124738 0.0115997 0.0179616 0.0140927 0.0130449 0.0104011 0.0146438 0.0114065 0.0157396 0.0135855 0.0128285 0.00754485 0.0305995 0.0181798 0.0192597 0.048465 0.0189101 0.0121396 0.00705945 0.0104833 0.00804011 0.0114006 0.00754285 0.00809562 0.00543453 0.00707061 0.0126759 0.0128725 0.0104646 0.021338 0.00769287 0.00642344 0.00572439 0.00467889 0.00776383 0.00501854 0.0044633 0.00535785 0.00198798 0.00168449 0.00151264 0.00165532 0.0014229 0.0012654 0.00145315 0.00129016 0.00136739 0.00136014 0.00162558];
+
+x = [0 -4.24346e-05 8.71535e-05 7.73439e-05 8.19646e-05 0.000129101 -0.000123236 -3.81299e-05 -9.68599e-05 -5.53157e-05 0.000115778 -2.39115e-05 0.000118613 0.00010519 3.61086e-05 -5.87477e-05 0.000160884 0.00621398 -0.0120009 0.00291489 0.003279 0.00576099 0.0150894 -0.00203845 0.000224768 0.00938943 -0.00811659 0.00119158 0.00746353 0.000121729 6.94127e-05 -0.00419329 -0.00568876 0.0139627 0.00477986 0.00962919 -0.0013915 -0.00812751 -0.00531474 -0.00482226 -0.00294706 -0.00801682 -0.0120678 -0.00846131 0.00427019 -0.0102197 -0.0124834 -0.0172146 -0.0103388 0.00629085 -0.0235327 0.00480739 -0.012399 0.00482727 -0.0133079 -0.0235706 -0.013214 -0.0103451 -0.0124942 -0.0113611 -0.0147532 -0.0156376 -0.0140083 -0.0117917 -0.00889636 -0.00979012 -0.0130616 -0.0115069 -0.00713748 -0.00853417 -0.0125016 -0.0152137 -0.0137736 -0.017015 -0.00826265 -0.011039 -0.010147 -0.0102702 -0.0115688 -0.00834822 -0.00480605 -0.00944234 -0.00613833 -0.00378726 -0.00531372 -0.00313342 0.0016912 -0.00846053 -0.000238551 0.00999922 0.00545356 0.00835875 0.00236814 0.00551584 0.0107257 0.0192157 0.00484362 0.0160764 0.0151525 0.00104963 0.0101495 0.0084537 0.00141839 0.00637473 0.0137394 -0.000386742 0.00881242 0.00421751 -9.91609e-05 0.00131641 -0.00112953 -0.00849616 0.00188975 0.00111134 0.000226679 0.00372954 0.0044746 0.000745515 0.00419199 0.00522998 0.00256299 0.00610309 0.00232603 0.00656134 0.00905603 0.0075174 0.00816977 0.00727421 0.0137445 0.00779005 0.00792792 0.00653672 0.00822117 0.00938434 0.00731794 0.0107173 0.00811708 0.0119406 0.00695679 0.00982725 0.0147284 0.0119065 0.0124385 0.0122512 0.0137476 0.0108564 0.0148077 0.00818148 0.00425363 0.00718716 0.00917207 0.00465425 0.00723085 0.00538666 0.00288388 0.00340082 0.00374876 -0.00419625 0.00672228 0.00246597 0.00614105 0.0041607 -0.00100455 0.000259257 0.00801794 0.00982123 0.00809857 0.00416525 -0.00185046 -0.00260187 0.0142119 0.0101674 0.0122865 0.00114529 0.000846205 0.00832562 -0.00102725 0.00684259 0.00459711 0.00405431 0.00127849 0.00401698 0.00291901 0.00223591 0.000508513 -0.000883496 -0.00164503 -0.00268374 -0.00238574 -0.00118393 0.000937702 -0.0055201 -0.0073194 0.0152312 0.011294 -0.0086854 -0.0147189 -0.00558232 -0.00394173 0.00198213 0.00608358 -0.0108543 0.00461987 -0.00406849 -0.00326745 -0.000672017 0.00256795 0.00272686 -0.00039404 0.000865531 0.0041187 0.00910968 0.00672891 0.00234949 0.00273352 0.00181897 0.00125234 0.00475895 0.00389612 0.00212937 0.00385255 0.00355574 0.00119965 -0.00219954 0.00283924 0.0045105 0.00317797 0.00898716 0.0114602 0.00378875 0.00679902 -0.00121378 0.00429642 0.00696563 0.0112035 0.000287787 -0.00228596 0.00212873 0.00858424 0.0097049 0.00384796 0.00699805 0.0061758 0.0122655 0.00828882 0.0117052 0.00224441 -0.000388189 0.00874184 0.01205 0.0113738 0.00235464 0.00850786 -0.00388995 0.0111763 0.000643106 0.00639816 0.00978384 0.0052487 0.0127941 0.00993185 0.00441308 0.00313138 -0.00533244 -0.0096972 -0.00973506 -0.00905878 -0.0100051 -0.00301525 0.00448708 0.00885024 0.0112419 0.0165715 0.00982144 0.0109893 0.011797 0.0129021 0.00862637 0.0115569 0.0131937 0.0185587 0.0220149 0.0183705 0.00855004 0.0065538 0.00464233 0.00377284 0.00377567 0.0098812 0.0093124 0.00208854 0.0107114 0.00588716 -0.00395249 -0.0138018 -0.00139743 0.00231474 -0.00210971 -0.00098593 -0.00416663 -0.000202776 -0.000503991 0.00291823 -0.000509565 -0.00013748 6.83721e-05 -6.62129e-05 -5.95589e-06 0.000163029 -2.94313e-05 -1.41226e-06 -0.000127159 0.00013134 -0.00025853];
+y = [0 0.000204328 -0.000359824 7.60799e-05 -9.37671e-05 -5.80152e-05 -5.49278e-05 0.000120296 0.000109765 0.000342621 -6.31708e-07 -0.000325124 2.26864e-05 0.000213688 0.000144319 0.000240986 0.000209789 -0.0086677 0.0129371 -0.00393705 -0.00240819 -0.00103602 -0.00633166 0.00905698 0.00757496 -0.00216864 0.0126588 0.00563069 0.000585525 0.0093867 0.0127259 0.0111814 0.0333927 0.00932608 0.0160619 0.0021804 0.0121537 0.00945414 0.0325899 0.0149477 0.0227223 0.0196483 0.0188425 0.029973 0.0479662 0.0196213 0.0120299 0.00944461 0.0119208 0.0183874 -0.00192976 0.0143755 -0.000162104 0.00960468 0.00514583 0.00164096 0.00554287 0.00691961 0.00311745 0.00258077 0.00452352 0.00261956 0.00523992 0.00732337 0.00983647 0.00289151 -0.00233132 0.00542192 0.00533552 0.00271515 0.00571588 -0.00239608 -0.000750886 0.00362278 -0.00334313 -0.00153819 -0.000611185 -0.00239244 -0.00338887 -0.000845023 -0.00546571 -0.000248397 -0.000156743 4.33485e-05 0.00194943 0.00384201 -0.00337436 0.00377249 0.00241832 0.00141299 0.00767455 0.00826595 0.0109429 0.0111026 0.00696724 -0.000415096 0.0110438 0.00361291 0.00107296 0.0129179 0.00114137 0.00249299 0.00862582 0.00377351 -0.00451957 0.00962203 -0.0015557 5.56536e-05 0.00136782 -0.00384426 -0.0067822 -0.00139039 -0.00507732 -0.0041727 -0.00435909 -0.00464438 -0.00542727 -0.00391032 -0.00389525 -0.00482716 -0.000177655 -0.00450206 -0.00320455 -0.00202606 -0.00690437 -0.00214849 -0.0044473 -0.00944357 -0.000894671 -0.00796149 -0.0046371 -0.00569116 -0.00547117 -0.00617576 -0.00381936 -0.00761608 -0.00664516 -0.00607395 -0.00741986 -0.00435547 0.000791578 -0.00294566 -0.00471792 -0.00947074 -0.00851501 -0.00565964 -0.00622709 -0.00840516 -0.00855547 -0.00568607 -0.00380322 -0.00484016 -0.00861393 -0.00668283 -0.00647052 -0.00811103 -0.00901533 -0.00616074 -0.00704351 -0.00571293 -0.0072006 -0.00452923 -0.00799317 -0.0106958 -0.010318 -0.00990502 -0.00847562 -0.00616359 -0.00389208 -0.0031538 -0.00541939 -0.00591785 -0.00397182 -0.00323923 -0.0043797 -0.00777217 -0.00716407 -0.00353097 -0.00364774 -0.0043592 -0.00272548 -0.00140546 -0.00137506 -0.00372805 -0.00316043 -0.0042664 -0.00533948 -0.00370825 -0.00949356 -0.00887874 -0.0106823 -0.0057666 0.00114163 -0.0238717 -0.0185981 0.000724678 0.00785414 -0.00517492 -0.00994796 -0.00950159 -0.0174094 0.00158522 -0.0131021 -0.00228096 -0.00421623 -0.00705121 -0.0104503 -0.0109667 -0.0102918 -0.00969282 -0.0110911 -0.00812833 -0.00159531 -0.0049659 -0.00643019 -0.0087362 -0.0100645 -0.00492639 -0.010123 -0.0101391 0.00277095 -0.00151114 -0.00212821 -0.0120646 -0.00629898 -0.00637808 -0.00718914 0.000731233 0.0133484 0.00803888 -0.0114201 -0.0030069 0.00310268 -0.00187037 0.000696273 -0.00652594 -0.00758808 -0.00539047 -0.00455091 -0.00298469 -0.00462718 -0.00434272 -0.0059528 -0.00445012 -0.00729047 -0.00572527 -0.010036 -0.0127272 -0.00523248 -0.00303387 -0.000822378 -0.00691088 -0.00660249 -0.0143434 -0.00203793 -0.0147616 -0.00603344 -0.00145005 -0.00892889 -0.000988243 -0.00505624 -0.00758437 -0.00707416 -0.00871257 -0.0121906 -0.0112068 -0.0108632 -0.0168984 -0.0152977 -0.00964994 -0.00796648 -0.00767111 -0.00484946 -0.00416775 -0.00416906 -0.000244006 -0.00435976 -0.00194637 0.00619571 0.000530164 -0.0129595 -0.00424634 -0.0075872 -0.00930663 -0.0115238 -0.00349045 -0.00358084 -0.00829199 0.00330293 -0.00697479 -0.000493523 -0.013539 -0.000709515 0.00552935 0.011406 0.00095419 -0.0025942 0.00235296 0.000760328 0.00441691 -0.000889965 0.000704515 -0.00292312 0.000802791 -0.00016234 -9.61823e-06 -1.9932e-05 0.000249597 3.93242e-05 -0.000204838 -5.2276e-05 0.000123329 -0.000438072 0.000151965];
+z = [0 -9.64658e-06 7.03945e-05 9.09483e-05 -0.000363336 -0.000193492 -0.000148889 0.000176356 -3.95041e-05 -7.41365e-05 -0.000638669 0.000226437 0.000136281 0.00015953 3.70733e-05 0.000581939 -0.000185053 0.000278609 -0.0039202 -0.00217225 -0.000538441 0.00511944 0.00411814 -6.7842e-06 0.00825446 0.0149711 0.0185738 0.0190497 0.0169868 0.0140352 0.0158413 0.00650451 0.0376733 0.014573 0.0217949 0.00371955 0.011917 0.0186783 0.0232513 0.0207271 0.00402345 0.0306006 0.00907937 0.00406955 0.0360196 0.0044247 0.0204511 0.0043904 0.00245758 0.0141277 0.00578564 -0.00518898 0.00735867 0.00863949 0.00066754 0.00645226 0.00660007 -0.00193774 0.0001358 0.00239511 0.00376228 -0.00560209 0.00468048 0.00514218 0.00427959 0.00962194 -0.00666892 0.00547005 0.0113137 0.00507462 0.0146809 -0.00753733 -0.00387708 0.000707309 0.00176431 -0.00205749 0.00436884 -0.00324613 0.00251866 0.000456466 0.00851096 -0.00379077 0.00780357 0.000613834 0.00198632 0.000343986 0.00882993 0.00645045 0.00365535 0.00520433 0.00646816 0.00627921 0.00254849 0.00404514 -0.000406334 -0.00153583 0.00284635 0.00095495 -0.000853335 0.00076378 -0.00310709 -0.000534566 0.000581196 -0.00187339 -0.00268137 0.000454393 -0.00326402 0.00179018 0.000687445 0.000546748 0.000607353 0.00388971 0.00168854 0.0028017 0.00227525 0.0043876 0.00392035 0.00228023 0.00298029 0.00317809 0.00610749 0.00420175 -0.00208311 0.00396719 0.0012112 -0.00268455 0.00281126 -0.0092204 0.0121282 -0.00560226 -0.000309605 0.00143829 -0.00334084 0.0039812 -8.42744e-05 0.00386651 -0.00361522 0.00143954 0.00190103 0.000522476 -0.0010711 -0.000751263 0.00468789 0.00365573 0.00291032 -0.000568013 0.000134285 0.00054685 -0.00054511 5.70824e-05 0.0035869 0.000179902 0.00419799 0.00451638 0.001585 0.00300293 0.00123728 0.000520918 0.00209513 0.00105725 0.00251548 0.000833801 0.00226301 0.000671691 0.0074015 0.00373656 0.00456477 0.00450154 0.00290425 -0.000299134 0.00107214 0.0023758 0.0054655 0.00332319 0.00425824 0.000836918 0.00776742 0.000956997 0.00232601 0.00316827 0.00522162 0.00109602 0.00303583 -0.000750361 -0.0023667 0.00120399 0.00126766 0.00083609 -0.00235571 8.67329e-06 0.00230422 0.00413383 0.008764 -0.0066263 0.000925412 0.00556172 0.00621337 0.00197899 -0.0026832 0.00249049 -0.0031991 0.0128274 -0.00813766 0.00700039 0.00605056 -0.000924999 0.00121924 0.00762761 0.000906501 0.00607177 0.00741256 0.00571461 0.0019757 -0.000709287 0.00524741 0.00284413 5.00776e-05 0.00946186 0.0075079 0.00183606 -0.00227517 0.00471794 0.00372035 0.0018374 0.00802605 0.00185333 -0.0047035 0.00742628 0.0142569 0.00707371 -0.0115996 -0.00380477 0.00530422 0.00250482 -0.00317097 0.00267832 0.00227734 -0.00941005 -0.000350822 0.00204698 0.00552164 -0.000160836 -0.0034101 -0.00234024 -0.00373448 -0.00287087 0.0133406 0.00085251 -0.00541296 0.00120797 0.00224371 0.0042547 -0.00159053 0.00826554 0.00149697 0.00388531 0.00121759 -0.00269208 -0.00163956 -0.0115823 -0.000228293 -0.00521276 -0.006834 -0.00751079 -0.00512234 -0.00131513 -0.0102579 -0.00768944 -0.00858772 -0.00162672 -0.00753255 -0.00359731 -0.00636032 -0.00375445 -0.00828524 -0.00457094 -0.000285845 0.0057742 0.0151166 -0.000743146 -0.0124465 0.00427572 -0.01353 -0.0103443 -0.0173464 -0.010804 0.00524001 -0.0103347 -0.00637103 -0.01197 -0.00308789 -0.0122685 0.00841686 -0.0105636 -0.00169108 -0.00256534 0.00272505 0.000674399 -0.000668501 0.00244924 -0.000958637 0.000649232 -0.000521342 -0.000155389 0.00024492 6.96772e-05 0.000150486 -0.000123244 1.85501e-06 -0.000152994 -0.000187548 9.34349e-05 -0.000212637 0.000369026];
+
+roll = [0 -0.00697247 0.011439 -0.0037672 0.00391318 0.00124693 0.0030287 -0.00448122 -0.00347616 -0.0100247 0.00208189 0.0100625 -0.00181607 -0.011119 -0.00362276 -0.0116096 -0.00818997 -0.0343405 -0.706642 0.595677 0.203868 0.390061 1.04302 -0.154686 -0.177945 -0.212443 -0.9854 0.113863 0.279816 -0.546618 -0.436199 -2.37908 -2.08871 0.0553227 -0.510254 0.180382 -0.958035 -1.40451 -2.38267 -1.12667 -1.75796 -1.13053 -2.63525 -2.60907 -3.58601 -2.52121 -1.79654 -2.25613 -1.33478 -0.732903 -0.9298 -1.47892 -0.380567 -2.02699 -2.13208 -2.24147 -2.18517 -2.52275 -1.79502 -1.37363 -1.88748 -1.03175 -1.65507 -1.38477 -0.399072 -0.40913 -1.59339 -1.60899 -0.863818 -0.991065 -1.57005 -1.64782 -1.7006 -2.22423 -0.83406 -1.46165 -0.843498 -0.850411 -1.12993 -1.07788 -1.37277 -2.00294 -1.33077 -1.40567 -1.8182 -1.64814 -1.8247 -1.90814 -1.44697 -1.8769 -1.93877 -1.46014 -0.793834 -0.629669 -0.301444 -1.32545 -1.30499 -1.20415 -1.4845 -1.58845 -1.34412 -1.74224 -2.00197 -1.53316 -1.68767 -2.06981 -1.59841 -1.49462 -1.48013 -1.15663 -1.35156 -1.39071 -1.25367 -1.49356 -1.44043 -1.13617 -1.15862 -1.05363 -0.743625 -1.10127 -1.00328 -0.573638 -0.618629 -1.68267 -1.02222 -1.2801 -0.653413 -0.506172 -1.2871 -0.495107 -0.361678 -0.493461 -0.762048 -1.4813 -0.441775 -0.531445 -0.551943 -1.37474 -1.5587 -1.70981 0.208783 0.0606307 -0.265095 -0.676214 -1.16917 -1.69928 -1.38808 -0.686836 -0.478791 -0.543355 -0.951801 -1.10595 -1.32134 -1.09503 -1.2005 -1.41269 -2.37385 -1.00221 -1.36333 -0.722503 -0.800073 -0.199508 -1.32356 -1.11201 -0.803518 -0.372442 -0.699338 -0.716352 -0.178544 -0.627999 -1.17488 -1.45338 -1.70703 -1.46451 -1.83811 -1.37418 -1.36829 -1.56187 -1.8804 -2.02195 -1.23298 -1.32484 -1.36442 -1.19876 -1.05828 -0.79918 -1.14581 -1.26577 -1.6265 -1.91479 -1.76773 -1.67093 0.0926437 0.0319117 -0.333926 -1.41818 -1.17289 -1.52312 -1.84482 -1.38148 -1.46084 -3.27883 -0.549232 -1.75344 -1.06523 -0.537597 -0.630611 -0.969317 -1.32225 -1.92723 -1.27726 -1.5211 -1.12751 -0.428813 -0.807155 -0.574223 -0.952206 -1.10544 -1.34586 -1.65117 -1.62982 -1.53227 -1.05804 0.0488398 -0.953483 -1.00224 -0.657381 -0.868573 -1.65495 -1.79803 -1.18743 -0.0486594 -1.21401 -1.72776 -1.49407 -1.17884 -1.25465 -1.73811 -0.952386 -0.668799 -0.78184 -1.38569 -0.832198 -1.10262 -0.931131 -0.717825 -1.14293 -1.34193 -1.47225 -1.24035 -0.255386 -0.51049 0.249707 -0.578932 -0.112682 -0.649949 0.0897079 -0.161068 -0.540194 -0.162269 -0.422191 -0.454308 0.575092 0.000792985 0.591403 -0.220016 -0.0570524 -0.460092 -0.851059 -0.544888 0.265254 -0.165818 0.95184 0.00759833 -0.523103 -0.0706023 1.23796 0.403462 -0.0332513 1.4473 3.71438 1.40783 2.70682 1.59649 0.845672 0.921996 0.840801 1.86878 2.44023 2.72089 1.05189 1.47246 1.07162 0.470408 -0.573698 -0.780683 -0.517149 0.180919 -0.136984 -0.328051 -0.739093 0.0983447 0.021223 -0.0187313 0.00106841 -0.00111421 0.00227972 -0.00775409 -0.0021896 0.00944541 0.00373123 -0.0038741 0.0147257 -0.00457401];
+pitch = [0 -0.00229179 0.000499854 -0.00071162 0.003954 -0.00227537 0.000171042 -0.00431982 0.00310028 0.000464334 0.00325114 0.00218017 -0.000226423 -0.00666426 0.0040347 -0.00572827 -0.00252566 0.409739 -0.106965 0.422755 0.6149 1.00093 0.85863 -0.440056 -0.732248 -0.618373 0.675355 -0.377637 0.42139 0.455565 -0.306133 -0.0858105 0.380259 -0.817764 -0.464705 -0.382899 -0.608065 -0.081234 1.74275 -0.573262 0.260839 0.622831 0.783492 0.259257 -0.153289 0.124555 -0.161443 -0.327383 0.752554 0.665943 -0.633409 1.04821 2.11617 0.263793 0.429431 0.126911 0.784859 0.749246 -1.7759 -0.326125 -0.40169 0.583243 0.757008 0.654661 0.669717 0.969141 1.39932 2.11495 1.05803 0.175636 -0.126807 0.124209 1.24958 1.48403 0.71194 0.779027 0.970415 0.41065 0.897172 1.12004 1.26689 1.20792 0.821871 0.860311 0.837231 0.867408 0.530834 0.882044 1.03845 1.19206 0.918048 0.832736 0.611604 0.683121 0.636224 0.744393 1.03378 1.01271 1.49852 1.15168 0.943035 1.20495 0.865091 0.951612 1.21074 1.52488 1.21162 0.850206 0.572902 0.630126 0.684501 0.455256 0.623057 0.505167 0.449704 0.582241 0.374617 0.260643 -0.0204377 0.0256043 -0.26454 -0.319661 -0.0751296 0.32799 0.196522 0.0518103 0.382681 0.3405 0.347285 0.39426 0.388134 -0.339197 0.704119 0.272995 0.281863 -0.424403 0.346723 1.45742 0.276316 0.688671 0.321258 0.0583006 0.140622 0.94608 0.726127 0.757012 1.47441 1.67256 1.41073 0.889791 1.05242 1.17434 1.42673 1.1505 0.668725 0.474379 -0.85654 0.404712 0.177909 0.735431 0.507653 1.32758 0.992634 0.730767 1.74019 1.00772 0.948151 0.836323 0.941635 0.119619 0.158425 0.895213 0.644582 0.152967 0.799178 0.785468 1.37186 0.67333 0.512899 0.746732 -0.109947 0.563172 0.222358 -0.353246 -0.539362 -1.02317 -1.23095 -0.34159 1.01101 1.54783 1.71109 1.26118 1.7363 0.387466 0.254116 -0.090165 -0.0951025 -0.03808 0.947861 0.364725 0.237979 0.0200293 -0.0453891 0.31848 0.916517 0.691965 0.577344 0.63224 0.072274 0.687555 0.754695 0.727415 0.412926 0.368177 0.320089 0.298686 0.352057 0.0202537 -0.0360367 0.714147 0.151168 -0.131895 0.790423 0.397934 0.207399 0.210421 0.553233 0.136282 0.361315 0.694285 1.19553 0.346202 0.820755 0.913812 0.96262 0.531157 0.252745 0.317119 0.575038 0.752224 0.505492 0.900339 0.0876113 0.77845 0.673826 0.298813 -0.220698 0.140829 0.224464 0.169208 0.597158 -0.0845539 0.82377 -0.85227 0.197967 0.231534 -0.0349389 -0.136615 0.444754 -0.198224 -0.530315 -0.873437 -0.293603 0.0188371 0.415711 -0.536511 0.251122 -0.0210569 0.354703 -0.301084 0.359748 -0.204457 -0.340254 -1.98404 -0.14539 -0.278535 -1.04035 -0.646792 -0.353741 0.198721 -0.698947 -0.253543 -0.479373 -0.998304 0.329982 -0.0150488 -0.232996 0.11367 -0.263232 -0.656289 -0.316895 0.235207 0.276941 0.805215 -0.739925 -0.018484 -0.268712 0.115287 -0.0447731 -0.00582324 -0.0905591 0.0474295 0.0073314 -0.00392141 -0.000782993 0.00241239 -0.00109896 0.00113237 -0.00180142 0.0030126 0.00394887 0.0014572 0.000337865 7.99176e-05];
+yaw = [0 -0.000161686 0.0032161 0.00257986 -0.0148895 -0.00775527 -0.0066706 0.00822511 2.74797e-05 -0.00120073 -0.0232415 0.0100288 0.00670114 0.0100244 0.00112335 0.0232722 -0.00544454 -0.430297 0.197413 0.318077 1.19285 0.824785 0.522653 -0.166473 0.697412 -0.127265 1.36169 0.831942 0.756734 -0.247735 -0.508298 -0.983596 0.353067 0.215551 0.432429 0.216151 0.0397583 0.627661 1.3034 0.171752 -0.0724861 1.29244 0.495153 -1.94984 1.68422 -0.493211 0.499761 0.585196 1.1265 1.43676 0.991768 0.831315 0.977594 0.840594 -0.354331 0.778799 0.66594 0.564547 1.24686 1.49184 1.34736 -0.208165 0.506705 0.283346 0.599432 0.850818 0.869317 0.913312 0.614873 0.79196 -0.0188216 -0.247626 -0.239266 0.309909 -0.0584155 -0.0377374 -0.028739 -0.746392 0.0755308 0.181368 0.118504 -1.02735 -0.13515 -0.331222 -0.567641 0.329769 -0.296064 -0.974358 -0.140646 0.244365 0.25554 -0.163399 0.0851915 0.258937 0.0142945 -0.625995 0.113146 -0.307074 -0.440209 0.164854 -0.187281 -0.107339 -0.605807 0.194123 -0.0617479 -0.0317805 0.0771855 -0.0184895 -0.708873 -0.27359 -0.497444 -0.420419 -0.496697 -0.503741 -0.788029 -0.586933 -0.692232 -1.20842 0.278887 -0.346126 0.0570406 -0.124973 0.309732 -0.524967 -0.139757 0.115098 0.0836262 -0.443537 1.08471 0.17619 -0.271993 -0.219413 -0.777199 -0.302626 -0.368529 0.765426 0.194521 0.714603 -0.390747 -0.144703 0.85836 0.996679 0.524187 0.34606 0.381368 -0.418374 0.51134 0.0741821 0.453409 0.575322 -0.204904 -0.129898 -0.550175 -0.449117 -0.364079 -0.551881 -0.539426 -0.195489 -0.376189 0.0524624 0.0870607 0.373965 -0.592063 0.120945 0.805568 0.30351 -0.387133 -0.64074 -0.0218592 -0.0372922 -0.151495 0.136863 -0.766291 -0.90385 -1.13801 -0.865704 -0.0904608 -1.47664 -1.27541 -0.735828 -0.354242 -0.384258 -0.638418 -1.00881 -0.614472 -0.486547 -0.274801 0.132622 0.715475 1.61421 0.940596 0.709336 1.19849 -0.00257673 -0.180559 -1.08725 -0.636779 -0.649876 -0.409159 -0.192793 -1.02339 -0.510792 0.224068 0.597195 1.54105 0.535605 0.723087 0.730369 -0.371158 0.275971 0.144074 0.302977 0.285828 -0.883684 -0.360669 0.0180366 -0.292566 0.188446 -0.140343 -0.407566 -0.662773 -0.6005 -1.12309 -0.648977 -0.477609 0.0492447 -0.322557 -0.273662 0.253909 0.100575 -1.09786 0.154206 1.03418 0.572754 0.12635 -0.0903762 0.0308006 0.131756 0.0105281 -0.491536 -0.141761 -0.94862 0.414641 0.271906 0.604514 0.427947 -0.0365438 0.277222 0.0283782 0.657427 0.724527 -0.295281 0.22394 -0.858543 0.172253 -0.0814776 0.0564259 0.118736 -0.283271 0.592259 0.62106 0.258227 0.0823895 0.133218 -0.114389 0.159368 0.41058 0.408956 -0.23241 0.0399566 -0.378538 -0.470384 0.14432 -1.03465 -0.46647 -0.387396 -0.81182 -1.71838 -0.2637 -0.897728 -1.75969 1.10463 -1.08475 -1.08329 -0.132061 0.0132544 -0.238716 -0.390951 -1.36424 -2.35338 -1.49625 -0.684186 0.293722 -0.105389 -1.0492 -0.316408 -0.792347 0.0951415 0.176206 0.325846 -0.0698992 -0.100712 0.0210328 -0.00260751 0.0101087 0.00475391 0.00586795 -0.00439265 -0.000228314 -0.00558726 -0.00652761 0.00306191 -0.00700772 0.0135158];
+
+roll = roll   * pi / 180; % to radian
+pitch = pitch * pi / 180; % to radian
+yaw = yaw     * pi / 180; % to radian
+
+
+%parameters
+n = 400;
+noiseT = 0.002;
+lambdaT = 100;
+noiseR = 0.002;
+lambdaR = 100;
+
+%filter
+x_filtered = pf_filter(x, n, noiseT, lambdaT);
+y_filtered = pf_filter(y, n, noiseT, lambdaT);
+z_filtered = pf_filter(z, n, noiseT, lambdaT);
+roll_filtered = pf_filter(roll, n, noiseR, lambdaR);
+pitch_filtered = pf_filter(pitch, n, noiseR, lambdaR);
+yaw_filtered = pf_filter(yaw, n, noiseR, lambdaR);
+
+%show
+figure
+subplot(4,1,1)
+plot(index,x,'b', index,x_filtered,'r');
+legend('x', 'x filtered');
+subplot(4,1,2)
+plot(index,y,'b', index,y_filtered,'r');
+legend('y', 'y filtered');
+subplot(4,1,3)
+plot(index,z,'b', index,z_filtered,'r');
+legend('z', 'z filtered');
+subplot(4,1,4)
+plot(index,stddev,'b');
+legend('stddev');
+
+%show
+figure
+subplot(4,1,1)
+plot(index,roll,'b', index,roll_filtered,'r');
+legend('roll', 'roll filtered');
+subplot(4,1,2)
+plot(index,pitch,'b', index,pitch_filtered,'r');
+legend('pitch', 'pitch filtered');
+subplot(4,1,3)
+plot(index,yaw,'b', index,yaw_filtered,'r');
+legend('yaw', 'yaw filtered')
+subplot(4,1,4)
+plot(index,stddev,'b');
+legend('stddev');
+
+

Matlab/ParticleFilter/test_kinect.m~

@@ -0,0 +1,50 @@
+
+
+% signals
+x = [0 -7.17718e-06 0.000149943 -0.000276212 0.000118147 0.000132833 -7.68572e-05 -0.000388181 6.57036e-05 0.000244131 -0.000265382 0.000674275 -6.0332e-05 0.000352076 0.00041996 -0.000758339 0.00210934 0.000399089 -0.000409156 0.0047982 0.0039244 0.00435027 0.00485974 0.00346061 0.0018604 -0.000905861 0.00250076 0.00214402 0.000318011 -0.00352464 0.00774855 0.00641464 0.00011028 0.00181151 -0.00313881 -0.00159122 0.000872649 -0.00925038 -0.0109046 -0.0279911 -0.00284128 -0.00634648 -0.00987577 -0.00809708 0.00135329 0.00141078 -0.00508487 -0.00524154 -0.0157128 -0.0154952 -0.00648952 -0.011292 -0.00702953 -0.0134704 -0.0102974 -0.0237573 -0.0113637 -0.0136848 -0.0134357 -0.0167649 -0.00662601 -0.00718927 -0.0167545 -0.0117351 -0.00313139 -0.0128256 -0.00886583 -0.00601757 -0.00631785 -0.0136913 -0.0130796 -0.00640869 -0.000587583 -0.00776267 -8.30889e-05 -0.00764275 -0.0047673 -0.00250125 0.00450075 -0.00641263 -0.000849128 0.00847131 0.00656557 0.0119401 0.0175035 0.0104212 0.00938523 0.00605232 0.00872052 0.01063 0.00795197 0.00730991 0.00414711 0.00778383 0.0057314 0.00532299 0.00678048 0.00635234 0.00429028 0.00268266 0.00285921 -0.00125447 -0.00343326 -0.00295475 0.00206432 0.00212367 0.00511998 0.00407538 0.00399027 0.00342568 0.00493171 0.00332177 0.00336831 0.00544102 0.00988577 0.00802416 0.00964469 0.0067216 0.00695488 0.0103022 0.0071584 0.00841331 0.00945374 0.00898707 0.00970355 0.00735274 0.00824642 0.00641495 0.00757965 0.00610715 0.00713819 0.00928026 0.012055 0.0105106 0.0118662 0.0122392 0.0104792 0.00808734 0.00854826 0.00684047 0.0085988 0.00592375 0.0052588 0.00384319 0.00372607 0.00494432 0.00475228 0.00364202 0.00258315 0.00617284 0.00378108 0.00530612 0.00723338 0.00106525 -0.000163257 0.00137579 0.00218695 -0.0012542 0.00378215 0.002096 0.00185335 0.00194138 0.00380033 0.0037328 0.00214076 -0.000261605 0.00554895 0.00190693 0.00482333 0.00412196 0.00433248 0.0032922 0.00149733 -0.00198263 -0.00465655 -0.00101215 -0.00452882 -0.00389808 0.00365704 0.00196409 -0.00150266 0.00132278 7.86781e-06 -0.000436306 -0.000997692 -0.00151774 -0.00290582 -0.000986993 -0.00202984 -0.00306979 -0.000241861 -0.0023663 -0.000143617 -0.000616923 0.00071498 -0.00136444 0.000806952 0.00092167 0.00274599 0.000827327 0.00379314 0.00362612 0.0028308 0.00371683 0.00211945 0.000794172 0.00338793 0.00358349 0.00317407 0.00381386 0.00329965 0.0061408 0.00434172 0.000996351 0.00116277 0.00479227 0.00521219 0.00549781 0.00172538 -0.000565588 0.00500929 0.00481606 0.0127962 0.00188589 0.00616825 0.00509858 0.00305247 0.00618845 0.000248432 0.00634307 0.00892508 0.0057171 0.00271344 0.00343686 0.0140943 0.00703895 0.00574613 0.0124045 0.00739682 0.00651699 0.020498 -0.0110877 0.00433773 0.0106311 0.00961483 0.0140001 0.00312042 0.0108534 0.00135618 0.00830334 0.0153873 0.0108157 0.0169969 -0.00464851 0.00816596 0.0118423 0.00561047 0.00855923 0.00718778 0.0125443 0.00616348 0.00718147 0.00534147 0.00167203 -0.00419921 -0.00742251 -0.00552565 -0.00556844 -0.0102499 -0.0138872 -0.0103608 -0.00935405 -0.00743747 -0.00296772 -0.00247735 0.00845826 0.00505942 0.00908333 0.013812 0.00857067 0.0182686 0.00592947 0.0126474 0.00578821 0.0194814 0.00121719 0.0182926 0.0109192 0.0115457 0.014065 0.00213802 -0.0102426 0.00826228 0.00567901 0.0131235 0.0350397 0.0167757 0.0172057 0.0183465 0.0198563 0.0193069 0.01778 0.0103664 0.00986159 0.00473499 0.00137529 0.00420779 0.00812897 0.000113249 0.00592332 0.00339369 0.0012721 0.010083 0.00799991 0.00702102 0.00649881 0.0030404 0.00210004 -0.00165895 0.00292256 -0.00186083 0.00441258 0.00263329 -0.002474 4.10676e-05 0.000647455 -0.00121567 -0.000948012 0.000322014 0.000219762 -0.00038138 0.000393793 0.000276357 -0.000241026 -0.00152412 0.000302628 -0.000860468 -0.000610992 0.000937909 0.00117072 -0.000948384 -0.000560746 0.000261694 0.000298828 5.32866e-05 -0.000208184 -0.000209108 -0.000162363 -0.000302538 -0.000584394 0.000218138 -0.000334874 0.000398353 -0.000544533 0.0006098];
+y = 1;
+z = 1;
+
+roll = 1;
+pitch = 1;
+yaw = [0 0.0138625 -0.0205169 0.0508271 -0.03702 0.0216298 -0.0071385 0.0486512 -0.0353205 0.0356788 0.0621803 -0.0675504 -0.0610086 0.0191248 -0.0686484 0.078647 -0.215955 0.222853 0.535986 -0.157076 0.239414 0.648483 -0.00482519 -0.0347738 -0.857926 -0.185939 -0.0811504 -0.333544 -0.922141 -1.57654 -0.104422 -0.212015 -1.09698 -1.808 -1.44291 -1.66115 -1.19908 -2.86579 -2.17578 -2.45784 -0.599695 -1.24934 -1.20622 0.219747 -0.348318 -1.18928 -0.53417 -1.82522 -2.2156 -2.53528 -2.64981 -1.21297 -1.67351 -1.94906 -1.13474 -1.1618 -0.51443 -0.27769 -1.8986 -1.57831 -1.25621 -0.86068 -1.6979 -1.55513 -1.92766 -2.04434 -0.852749 -0.95132 -1.20863 -0.748142 -1.04782 -1.19536 -1.37872 -1.89795 -1.37617 -1.31626 -1.93325 -1.511 -1.98064 -2.62878 -1.99733 -1.65566 -1.88769 -1.49137 -1.24202 -1.08672 -0.297613 -1.55247 -1.24222 -1.3313 -1.62514 -1.51867 -1.40812 -1.58506 -1.80194 -1.57594 -1.90908 -1.70978 -2.04307 -1.28751 -1.33265 -0.819239 -1.36529 -1.11071 -1.51813 -1.44329 -1.19389 -1.21335 -1.15439 -1.07836 -0.652882 -0.716911 -0.64296 -1.20633 -1.55259 -0.952261 -1.16282 -0.571938 -0.966765 -1.18008 -0.336663 -0.682695 -0.839598 -0.590307 -1.31757 -0.372847 -0.334298 -0.542365 -1.82461 -1.38264 -1.54329 -0.474113 0.131601 -0.165138 -0.712546 -1.3513 -1.48896 -1.70229 -1.11744 -1.26407 -0.898101 -0.475791 -0.505334 -0.911445 -1.05962 -1.34112 -1.11278 -1.09297 -2.30311 -1.36669 -1.50016 -0.731569 -0.974022 -1.34955 -0.962003 -0.724183 -0.492458 -1.18278 -0.0750827 -0.101046 -1.20587 -1.46872 -1.70565 -1.60328 -1.83232 -2.88432 -1.32855 -1.45884 -1.94864 -1.21744 -1.36144 -1.46579 -1.19324 -0.777264 -1.14617 -1.4781 -1.74714 -2.0015 -1.77974 -1.7534 -0.743309 -0.598297 -0.35454 -0.539937 -0.557158 -1.05504 -0.858589 -0.894771 -1.53595 -1.7775 -1.42647 -1.7212 -2.99655 -0.463739 -1.71624 -1.23245 -0.813187 -0.416194 -0.669693 -1.22522 -1.88152 -1.80743 -1.10323 -0.916615 -0.794212 -0.918734 -0.706259 -0.961836 -0.99244 -1.34899 -1.69983 -1.23063 -0.883386 -1.08027 -0.882478 -1.09176 -0.65819 -0.790655 -0.866973 -1.47657 -1.50859 -1.29712 -0.764263 0.0414162 -1.15544 -0.753635 -1.30834 -0.898253 -1.30238 -1.43098 -0.56996 0.115919 -1.03715 -1.0479 -1.21217 -1.05506 -1.07723 -1.2435 -0.484735 -0.48917 -1.23569 -1.49113 -1.37683 -1.7992 -0.595289 -0.729136 -0.59366 -2.04639 -0.0944586 0.335957 -0.773459 0.644048 -0.417065 -0.951768 -0.705456 0.0222041 -0.144679 -0.64648 -0.064453 0.627685 -0.529511 -0.529965 0.21694 0.53353 0.133591 0.0725028 0.349745 0.0435301 -0.00884339 -0.0296694 0.0036566 0.217436 -0.526511 -0.512979 -1.32967 -0.491154 0.224525 0.609691 -0.30598 1.04681 1.11591 -0.342271 0.181812 1.00252 -0.569827 -0.174485 0.621023 1.15114 0.85475 1.17868 0.418458 0.132773 0.09667 0.105433 3.05352 3.43917 1.56984 1.66762 1.91426 2.8391 2.61215 2.92371 1.4164 1.0191 0.490665 0.341318 1.48112 0.901748 0.991522 1.52663 1.06743 1.60625 2.42876 2.18304 1.5142 1.05712 1.21268 1.40639 -0.539377 1.00853 -0.254121 0.662136 0.29893 -0.0109695 0.387949 -0.573484 -0.838396 -0.198825 0.040522 -0.29886 -0.368194 0.130778 -0.474338 -0.762008 0.0665195 0.155289 0.0655191 -0.0332738 0.0774652 -0.000751732 0.0230952 0.0195789 -0.000320425 -0.00322251 -0.026999 0.000401567 0.0640858 -0.0439683 0.0527193 0.00422349 -0.0218244 0.0171353 -0.0126251 -0.0419359 0.03175];
+roll = roll   * pi / 180; % to radian
+pitch = pitch * pi / 180; % to radian
+yaw = yaw     * pi / 180; % to radian
+
+
+%parameters
+n = 400;
+noiseT = 0.005;
+lambdaT = 100;
+noiseR = 0.005;
+lambdaR = 150;
+
+%filter
+x_filtered = pf_filter(x, n, noiseT, lambdaT);
+y_filtered = pf_filter(x, n, noiseT, lambdaT);
+z_filtered = pf_filter(x, n, noiseT, lambdaT);
+roll_filtered = pf_filter(roll, n, noiseR, lambdaR);
+pitch_filtered = pf_filter(pitch, n, noiseR, lambdaR);
+yaw_filtered = pf_filter(yaw, n, noiseR, lambdaR);
+
+%show
+index = 1:length(x);
+
+figure
+plot(index,x,'b', index,x_filtered,'r');
+hold on;
+plot(index,y,'c', index,y_filtered,'m');
+plot(index,z,'g', index,z_filtered,'y');
+legend('x', 'x filtered', 'y', 'y filtered', 'z', 'z filtered')
+
+%show
+figure
+plot(index,roll,'b', index,roll_filtered,'r');
+hold on;
+plot(index,pitch,'c', index,pitch_filtered,'m');
+plot(index,yaw,'g', index,yaw_filtered,'y');
+legend('roll', 'roll filtered', 'pitch', 'pitch filtered', 'yaw', 'yaw filtered')
+legend('yaw', 'yaw filtered')
+
+

Matlab/ParticleFilter/test_odometry.m

@@ -0,0 +1,28 @@
+
+
+clc
+clear all
+close all
+
+% position (x)
+x=[0 0.0958093 0.102248 0.121139 0.14751 0.168275 0.180045 0.189047 0.203946 0.213641 0.22573 0.243683 0.245992 0.254727 0.260212 0.246672 0.259118 0.273364 0.295793 0.317168 0.319033 0.330263 0.291336 0.342969 0.373641 0.406199 0.451661 0.49569 0.52575 0.558851 0.595505 0.617386 0.635253 0.663907 0.695187 0.721908 0.748228 0.775707 0.798103 0.817422 0.81696 0.834573 0.859951 0.866837 0.861447 0.859494 0.863317 0.86665 0.856142 0.861513 0.869291 0.86055 0.858222 0.850158 0.86469 0.853298 0.849211 0.85666 0.846806 0.834052 0.818975 0.816591 0.818847 0.812942 0.804087 0.802885 0.799035 0.793771 0.782671 0.783806 0.753611 0.735965 0.718761 0.70112 0.68173 0.637936 0.59822 0.570844 0.546408 0.503599 0.481603 0.481476 0.468762 0.468941 0.464934 0.457286 0.468546 0.449624 0.423916 0.398509 0.448139 0.463816 0.524683 0.543314 0.585886 0.629963 0.642661 0.689202 0.733252 0.720786 0.747091 0.769375 0.796756 0.804347 0.805587 0.792853 0.785854 0.791591 0.775623 0.772854 0.774293 0.775352 0.778641 0.773735 0.763698 0.770237 0.764968 0.782758 0.790375 0.794311 0.801609 0.807287 0.817432 0.834678 0.854601 0.860135 0.857215 0.876742 0.880283 0.885587 0.895356 0.901144 0.891045 0.904938 0.88169 0.875238 0.873543 0.879889 0.858548 0.848491 0.845521 0.832916 0.82651 0.817256 0.81536 0.807806 0.80487 0.789718 0.788688 0.790668 0.786018 0.783837 0.776029 0.766569 0.76899 0.772462 0.753362 0.75065 0.760805 0.771717 0.752581 0.777233 0.771257 0.784665 0.789536 0.783277 0.768278 0.775663 0.782527 0.801327 0.773798 0.783786 0.782585 0.777043 0.765654 0.757504 0.75159 0.745206 0.745253 0.694793 0.657977 0.627305 0.587667 0.558428 0.507883 0.429494 0.354626 0.276308 0.221929 0.216919 0.222186 0.2257 0.21829 0.21727 0.220388 0.234087 0.271358 0.365188 0.397868 0.464485 0.436238 0.473228 0.516054 0.581783 0.66704 0.702402 0.772848 0.836386 0.870549 0.883748 0.890929 0.9015 0.933377 0.993732 1.01464 1.01678 1.01107 1.00868 1.01584 1.0091 1.01149 1.00289 0.992969 1.00104 0.999807 1.00424 1.00244 1.00696 0.999669 0.989008 0.995648 0.977475 0.976959 0.986356 0.969375 0.973117 0.970714 0.96257 0.956754 0.954274 0.92185 0.935669 0.933797 0.918595 0.86588 0.831554 0.800549 0.77171 0.756608 0.738918 0.707971 0.681183 0.654234 0.644363 0.619473 0.607539 0.589974 0.569724 0.538563 0.524551 0.521722 0.497904 0.486697 0.453492 0.43853 0.449554 0.472133 0.481396 0.487145 0.49219 0.520493 0.56075 0.603805 0.616122 0.687576 0.726017 0.756402 0.862825 0.926746 0.974142 0.960781 0.925374 0.932887 0.938424 0.948403 0.924803 0.914451 0.920006 0.874211 0.873257 0.888927 0.905825 0.908478 0.932547 0.975009 1.03873 1.0638 1.06204 1.07596 1.07522 1.06945 1.05537 1.07162 1.03632 1.03053 1.02946 1.01674 1.0092 0.98566 0.979956 0.946627 0.933132 0.904111 0.826381 0.789019 0.738187 0.717317 0.656708 0.490356 0.434063 0.3134 0.213561 0.18305 0.174464 0.13647 0.127878 0.0663346 0.018491 -0.00133265 0.000999137 -0.00121855 0.000119434 0.000319056 3.22909e-05 -0.000269401 -0.000233193 0.00030071 -0.000469815 -5.96254e-05 0.000130806 9.13643e-05 2.98268e-05 4.07632e-05 7.35067e-05 0.0153078 0.0186766 0.0295282 0.0543363 0.0719927 0.086474 0.126864 0.161484 0.19345 0.300308 0.404477 0.422215 0.514335 0.513278 0.625498 0.93727 0.993653 1.03992 1.08643 1.1112 1.23404 1.22048 1.19735 1.20285 1.17902 1.17133 1.16618 1.13694 1.12139 1.10714 1.09385 1.08936 1.08 1.04605 1.03821 1.03905 1.02285 0.989178 0.935135 0.865405 0.723314 0.641447 0.602483 0.522911 0.491991 0.462587 0.500009 0.543585 0.668132 0.752387 0.782115 0.783924 0.772362 0.763335 0.683723 0.644116 0.627112 0.614968 0.57313 0.523073 0.435806 0.335873 0.269904 0.25352 0.260491 0.24324 0.325414 0.359984 0.411085 0.348953 0.272724 0.0950071 -0.00396737];
+%filter
+x_filtered = pf_filter(x, 400, 0.07, 15);
+%show
+figure
+index = 1:length(x);
+plot(index,x,'b', index,x_filtered,'r');
+legend('x', 'x filtered')
+
+% rotation (yaw)
+yaw=[0 0.367344 0.423404 0.640698 0.914954 1.12181 1.25882 1.36415 1.53533 1.68386 1.7685 1.984 1.98521 2.16754 2.31386 2.71056 2.92328 3.16512 3.30591 3.3121 3.39057 3.4986 3.38722 3.27962 2.94052 2.71029 2.14679 1.77782 1.1752 0.750571 0.374997 0.252442 0.0770365 -0.0714351 -0.0788259 -0.0948595 -0.0976391 -0.102182 -0.0721881 -0.0533192 -0.0312146 0.0153472 0.00734632 0.0203816 0.0274327 0.0244179 0.01258 -0.00401542 -0.0257598 -0.0261856 -0.030745 0.00809133 -0.0201015 -0.0173277 0.0202852 0.0392073 0.0418929 0.102132 0.114319 0.0860487 0.0639182 0.0709067 0.0550139 0.0589345 0.059498 0.0386182 0.0237123 0.0133304 0.0453462 -0.00725487 -0.0839458 -0.148932 -0.241809 -0.337385 -0.408145 -0.668709 -1.03016 -1.23355 -1.41645 -1.8998 -2.29763 -2.56527 -2.92972 -3.28132 -3.29212 -3.18606 -3.06397 -3.11467 -3.21917 -3.28397 -3.31198 -3.25927 -2.7551 -2.5298 -1.79784 -1.23766 -1.06653 -0.8034 -0.850659 -0.844026 -0.723554 -0.553498 -0.479462 -0.31188 -0.265967 -0.21797 -0.156605 -0.123484 -0.131099 -0.10232 -0.0613497 -0.109752 -0.125175 -0.129335 -0.0552938 -0.0475251 -0.0330476 -0.0433853 -0.000880761 0.115343 0.170772 0.131315 0.141837 0.0999629 0.121093 0.10641 0.0607733 -0.0309408 -0.109561 -0.066559 -0.0824674 -0.0320398 -0.0439917 -0.0623439 -0.0695493 -0.0444161 -0.0065631 0.057401 0.0976348 0.139955 0.19723 0.229909 0.191066 0.220196 0.264731 0.360177 0.346012 0.34643 0.351766 0.316502 0.367959 0.361346 0.403335 0.473158 0.519924 0.61397 0.64618 0.700117 0.700964 0.66132 0.5243 0.429438 0.456057 0.476463 0.407447 0.330845 0.331997 0.299632 0.234435 0.336388 0.294559 0.293602 0.27388 0.288828 0.275381 0.296273 0.263594 0.225263 0.186247 0.206763 0.172048 0.151194 0.154786 0.148777 0.132994 0.35366 0.471686 0.949712 1.25971 1.26477 1.3436 1.51096 1.752 1.74347 2.02086 2.12265 2.47172 3.06266 3.02326 3.0401 2.83478 2.6832 2.43665 1.51546 0.690584 0.442643 0.168241 0.0486004 0.0388517 0.063886 0.0594586 0.0671994 0.0716274 -0.00795655 0.00190975 0.0219664 0.0227349 0.0180559 0.026195 0.0434311 0.0426099 0.0737965 0.0520878 0.00251581 -0.057547 -0.0536053 -0.0872076 -0.0905081 -0.0275865 0.0106225 0.00939888 0.0564355 0.0535977 0.0664939 0.0494566 0.0104787 -0.0241714 -0.026226 -0.0377078 -0.0367821 -0.0307445 -0.00809363 -0.00967068 0.0169084 0.0220944 0.0305258 0.0240909 0.0441721 0.0568962 0.0938898 0.181372 0.425365 0.828993 0.970071 1.29186 1.49596 1.72977 1.9131 2.33526 2.67046 2.65711 2.77644 2.87891 2.92561 2.84321 2.86918 2.53234 2.38821 1.90655 1.70474 0.774424 0.242343 0.101759 0.0606305 -0.0168209 -0.0321093 0.0145107 0.0541553 0.0516316 0.0220795 -0.00039793 -0.0337128 -0.0595012 -0.0539847 -0.0388872 -0.635682 -1.3613 -1.84956 -1.77388 -1.23053 -1.08225 -1.05091 -1.00551 -0.779717 -0.0594095 0.0348849 0.0411509 -0.0188255 -0.0742438 -0.0677669 -0.0538894 -0.105211 -0.146425 -0.172514 -0.127626 -0.0157509 0.066493 0.055035 0.165349 0.141456 -0.0243789 -0.0492561 -0.108507 -0.436607 -0.421971 -0.397179 -0.342693 -0.298114 -0.136957 -0.0635116 -0.00672385 0.145058 0.134837 0.140056 0.299669 0.37401 0.256271 0.103959 0.00974883 -0.0162673 0.0043005 -0.00114822 0.011008 0.00846304 0.0198297 0.0207307 0.0143213 -0.000866516 0.00788459 0.0133711 -0.00467761 -0.000142124 -0.000778014 0.00123566 0.171455 0.267658 0.34448 0.719087 0.900269 0.958507 1.051 1.20626 1.3786 2.5974 3.06579 3.01829 3.05114 3.09404 2.95468 0.52717 0.138967 0.0976445 0.248206 0.247692 -0.0151054 -0.0351626 -0.0324181 -0.0302425 0.00564108 0.0495676 0.151858 0.0565908 -0.0921944 -0.0842249 0.0416879 0.0400479 0.0786246 -0.048536 -0.0493043 -0.0457081 -0.03454 -0.0353161 0.00897072 0.182576 0.686738 0.942024 1.22062 2.96135 2.9593 2.90432 1.59589 0.838234 0.57694 0.164348 0.107916 0.0222738 0.00653589 -0.0789186 -0.0262052 0.0161459 0.0682029 0.10532 0.00317246 -0.0800566 -0.0553356 -0.0542734 -0.0175716 0.344492 0.239568 0.117746 -0.269454 -0.184009 -0.436715 0.097191 -0.532786 -0.30076 -0.00929552];
+yaw = yaw * pi / 180; % to radian
+%filter
+yaw_filtered = pf_filter(yaw, 400, 0.005, 150);
+%show
+figure
+index = 1:length(yaw);
+plot(index,yaw,'b', index,yaw_filtered,'r');
+legend('yaw', 'yaw filtered')
+
+

Matlab/ShowLogs/getPrecisionRecall.m

@@ -16,6 +16,9 @@ if exist(GT_file, 'file')
     display('--- getPrecisionRecall ---');
     display(['Loading GroundTruth ''' GT_file ''' ...']);
     GroundTruth = imread(GT_file);
+    if max(max(GroundTruth)) == 1
+        GroundTruth=GroundTruth*255;
+    end
 else
     error(['The ground truth ''' GT_file '''doesn''t exist.'])
 end
@@ -56,8 +59,11 @@ if ~isempty(GroundTruth)
     end
     
     lc = sortrows(lc, -1);
-    
+
     GT_total_positives = sum(sum(GroundTruth == 255, 2) > 0)
+    if GT_total_positives == 0
+        error(['The ground truth ''' GT_file '''doesn''t have any white pixels!?'])
+    end
     
     %figure
     %plot(sum(GroundTruth > 0, 2)>0)
@@ -76,7 +82,7 @@ if ~isempty(GroundTruth)
         if id && sum(GroundTruth(lc(i,6), id)) > 0
             lc(i,5) = 1;
         end
-        
+       
         %Recall = Loop closures detected / GT loop closures
         PR(i,2) = sum(lc(1:i,5) & ~lc(1:i,7) & lc(1:i,2)) / GT_total_positives;
         
@@ -91,7 +97,11 @@ if ~isempty(GroundTruth)
         PR(i,4) = sum(lc(i,4) & lc(1:i,5) & ~lc(1:i,7) & lc(1:i,2)) / GT_total_positives;
         
         %Precision = Good loop closures / total loop closure detected
-        PR(i,3) = sum(lc(i,4) & lc(1:i,5) & ~lc(1:i,7) & lc(1:i,2)) / sum(~lc(1:i, 7) & lc(1:i,2) & lc(i,4));
+        if sum(~lc(1:i, 7) & lc(1:i,2) & lc(i,4)) > 0
+            PR(i,3) = sum(lc(i,4) & lc(1:i,5) & ~lc(1:i,7) & lc(1:i,2)) / sum(~lc(1:i, 7) & lc(1:i,2) & lc(i,4));
+        else
+            PR(i,3) = 0;
+        end
         
         if lc(i,4) && ~lc(i,5) && ~lc(i,7) && id && lc(i,1) >= LoopThr
             display(['False positive accepted! id=' num2str(lc(i,6)) ' with old=' num2str(id) ' (p=' num2str(lc(i,1)) ')'] )

Matlab/ShowLogs/importfile.m

@@ -8,6 +8,10 @@ function [Data] = importfile(fileToRead1)
 % Import the file
 rawData1 = importdata(fileToRead1);
 
+if isstruct(rawData1) 
+    rawData1=rawData1.data; 
+end 
+
 % For some simple files (such as a CSV or JPEG files), IMPORTDATA might
 % return a simple array.  If so, generate a structure so that the output
 % matches that from the Import Wizard.

Matlab/ShowLogs/showlogs.m

@@ -97,15 +97,21 @@ end
 % xlabel('Location indexes')
 
 figure
-plot((sum(LogF(:,2:7),2)+LogF(:,17))*1000);
+plot(LogF(:,1), 'g'); % to verify that we have all timings below
 hold on
-ylabel('Time (ms)')
-xlabel('Location indexes')
+if size(LogF, 2) == 21
+    plot((sum(LogF(:,2:7),2)+LogF(:,17)+LogF(:,21)));
+else
+    plot((sum(LogF(:,2:7),2)+LogF(:,17)+sum(LogF(:,21:26),2)));
+end
+ylabel('Time (s)')
+xlabel('Node indexes')
 meanTime = mean(LogF(:,1))*1000
-plot([1 length(LogF(:,1))], [700 700], 'r')
+plot([1 length(LogF(:,1))], [0.7 0.7], 'r')
+plot([1 length(LogF(:,1))], [1 1], 'k')
 %plot([1 length(LogF(:,1))], [350 350], 'r')
 %legend('Processing time', 'Time limit')%, 'Acquisition rate (1 Hz)')
-title('Processing time')
+%title('Processing time')
 
 maxTime = max(sum(LogF(:,2:7),2)+LogF(:,17))
 maxDict = max(LogI(:, 6))
@@ -183,22 +189,51 @@ ylabel('MB')
 xlabel('Location indexes')
 % -------------------------
 
-figure
-% subplot(211)
-H1 = plot(LogI(:,7));
-% hold on
-% H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
-title('Working memory size')
-meanWM = mean(LogI(:,7))
-ylabel('WM size (locations)')
-xlabel('Location indexes')
-% set(H1,'color',[0.3 0.3 0.3])
-% set(H2,'color',[0 0 0])
-% subplot(212)
-% plot(LogI(:,6));
-meanDict = mean(LogI(:,6))
-% ylabel('Dictionary size')
-% xlabel('Location indexes')
+if size(LogI, 2) >= 18
+    LTMsize = zeros(1,length(LogI(:,16)));
+    for i=1:length(LogI(:,16))
+        LTMsize(i) = sum(LogI(1:i,16) == 0);
+    end
+    LTM = LTMsize(end)
+
+    figure
+    % subplot(211)
+    H2 = plot(LTMsize, 'r'); % global graph
+     hold on
+    H1 = plot(LogI(:,7)); % WM
+    H3 = plot(LogI(:,17), 'g'); % Local graph
+    % H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
+    %title('Graph size')
+    legend('Global graph', 'WM', 'Local graph')
+    ylabel('Nodes')
+    xlabel('Node indexes')
+    %set(H1,'color',[0.3 0.3 0.3])
+    %set(H2,'color',[0 0 0])
+    %set(H3,'color',[0 0 0])
+    % subplot(212)
+    % plot(LogI(:,6));
+    meanWM = mean(LogI(:,7))
+    meanDict = mean(LogI(:,6))
+    % ylabel('Dictionary size')
+    % xlabel('Location indexes')
+else
+    figure
+    % subplot(211)
+    H1 = plot(LogI(:,7));
+    % hold on
+    % H2 = plot(1:length(LogI(:,7)), ones(length(LogI(:,7)),1).*mean(LogI(:,7)), 'r--')
+    title('Working memory size')
+    meanWM = mean(LogI(:,7))
+    ylabel('WM size (locations)')
+    xlabel('Location indexes')
+    % set(H1,'color',[0.3 0.3 0.3])
+    % set(H2,'color',[0 0 0])
+    % subplot(212)
+    % plot(LogI(:,6));
+    meanDict = mean(LogI(:,6))
+    % ylabel('Dictionary size')
+    % xlabel('Location indexes')
+end
 
 meanWordsPerSign = mean(LogI(:,5))
 %% -------------------------

RTABMapConfig.cmake.in

@@ -2,12 +2,72 @@
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
+#  RTABMap_CORE         - core library
+#  RTABMap_UTILITE      - utilite library
+#  RTABMap_GUI          - gui library (set if RTABMap is built with Qt)
 
 # Compute paths
 get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
 set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
 
-find_library(RTABMAP_CORE NAMES rtabmap_core rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-find_library(RTABMAP_GUI NAMES rtabmap_gui rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-find_library(RTABMAP_UTILITE NAMES rtabmap_utilite rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
-set(RTABMap_LIBRARIES ${RTABMAP_CORE} ${RTABMAP_GUI} ${RTABMAP_UTILITE})
+#core lib
+find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+
+IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE)
+   SET(RTABMap_CORE
+      debug ${RTABMap_CORE_DEBUG}
+      optimized ${RTABMap_CORE_RELEASE}
+   )
+ELSEIF(RTABMap_CORE_RELEASE)
+   SET(RTABMap_CORE ${RTABMap_CORE_RELEASE})
+ELSEIF(RTABMap_CORE_DEBUG)
+   SET(RTABMap_CORE ${RTABMap_CORE_DEBUG})
+ENDIF()
+
+#utilite lib
+find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+
+IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE)
+   SET(RTABMap_UTILITE
+      debug ${RTABMap_UTILITE_DEBUG}
+      optimized ${RTABMap_UTILITE_RELEASE}
+   )
+ELSEIF(RTABMap_UTILITE_RELEASE)
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE})
+ELSEIF(RTABMap_UTILITE_DEBUG)
+   SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG})
+ENDIF()
+
+set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
+
+#gui lib (OFF if RTAB-Map is not built with Qt)
+if(@CONF_WITH_GUI@)
+   find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@")
+   
+   IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE)
+      SET(RTABMap_GUI
+         debug ${RTABMap_GUI_DEBUG}
+         optimized ${RTABMap_GUI_RELEASE}
+      )
+   ELSEIF(RTABMap_GUI_RELEASE)
+      SET(RTABMap_GUI ${RTABMap_GUI_RELEASE})
+   ELSEIF(RTABMap_GUI_DEBUG)
+      SET(RTABMap_GUI ${RTABMap_GUI_DEBUG})
+   ENDIF()
+   
+   set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
+endif(@CONF_WITH_GUI@)
+
+#backward compatibilities
+if(RTABMap_CORE)
+   set(RTABMAP_CORE ${RTABMap_CORE})
+endif(RTABMap_CORE)
+if(RTABMap_UTILITE)
+   set(RTABMAP_UTILITE ${RTABMap_UTILITE})
+endif(RTABMap_UTILITE)
+if(RTABMap_GUI)
+   set(RTABMAP_GUI ${RTABMap_GUI})
+endif(RTABMap_GUI)

app/src/CMakeLists.txt

@@ -5,7 +5,11 @@ SET(headers_ui
 )
 
 #This will generate moc_* for Qt
-QT4_WRAP_CPP(moc_srcs ${headers_ui})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs ${headers_ui})
+ELSE()
+    QT5_WRAP_CPP(moc_srcs ${headers_ui})
+ENDIF()
 
 SET(SRC_FILES
     main.cpp
@@ -21,12 +25,14 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-INCLUDE(${QT_USE_FILE})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
-	${OpenCV_LIBS} 
-	${PCL_LIBRARIES}
+    ${OpenCV_LIBS}
+    ${PCL_LIBRARIES}
 )
 
 # rc.exe has problems with these defintions... commented!
@@ -67,6 +73,9 @@ ELSE()
   ADD_EXECUTABLE(rtabmap ${SRC_FILES})
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+ENDIF()
 
 IF(APPLE AND BUILD_AS_BUNDLE)
   SET_TARGET_PROPERTIES(rtabmap PROPERTIES
@@ -83,7 +92,7 @@ ENDIF()
 # Installation stuff
 #--------------------------- 
 INSTALL(TARGETS rtabmap
-        RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
         BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
       
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -157,7 +166,8 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
   # over.
   # To find dependencies, cmake use "otool" on Apple and "dumpbin" on Windows (make sure you have one of them).
   install(CODE "
-   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+   file(GLOB_RECURSE QTPLUGINS \"\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/${plugin_dest_dir}/plugins/*${CMAKE_SHARED_LIBRARY_SUFFIX}\")
+    set(BU_CHMOD_BUNDLE_ITEMS ON)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)

app/src/main.cpp

@@ -25,13 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <QtGui/QApplication>
+#include <QApplication>
 #include <QtCore/QDir>
 #include "rtabmap/utilite/UEventsManager.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/gui/MainWindow.h"
-#include <QtGui/QMessageBox>
+#include <QMessageBox>
 #include "rtabmap/utilite/UObjDeletionThread.h"
 #include "ObjDeletionHandler.h"
 
@@ -43,16 +43,23 @@ int main(int argc, char* argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
 
-	ULOGGER_INFO("Program started...");
-
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	MainWindow * mainWindow = new MainWindow();
 
+	UINFO("Program started...");
+
 	UEventsManager::addHandler(mainWindow);
 
 	/* Start thread's task */
-	mainWindow->showNormal();
+	if(mainWindow->isSavedMaximized())
+	{
+		mainWindow->showMaximized();
+	}
+	else
+	{
+		mainWindow->show();
+	}
 
 	RtabmapThread * rtabmap = new RtabmapThread(new Rtabmap());
 	rtabmap->start(); // start it not initialized... will be initialized by event from the gui
@@ -67,37 +74,14 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	ULOGGER_INFO("Killing threads...");
+	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	ULogger::setLevel(ULogger::kInfo);
-
-	ULOGGER_INFO("Closing RTAB-Map core...");
-
-	//Since we can't put the Rtabmap object in the MainWindow class,
-	//we pop up a message box indicating that the rtabmap object
-	// is being deleted (saving data to the database)
-	QMessageBox * msg = new QMessageBox(QMessageBox::Information,
-			QObject::tr("RTAB-Map is closing..."),
-			QObject::tr("The detector is saving the working memory to database (located in RTAB-Map's working directory)..."),
-			QMessageBox::NoButton,
-			mainWindow);
-	msg->setEnabled(false);
-	msg->setIconPixmap(QPixmap(":/images/RTAB-Map.ico"));
-	msg->setWindowIcon(QIcon(":/images/RTAB-Map.ico"));
-	msg->show();
-	UObjDeletionThread<RtabmapThread> delThread(rtabmap);
-	ObjDeletionHandler handler(delThread.id(), app, SLOT(quit()));
-	UEventsManager::addHandler(&handler);
-	delThread.startDeletion(1); // make sure that app-exec() is called before the deletion of the object
-	app->exec();
-
-	ULOGGER_INFO("Closing RTAB-Map gui...");
+	UINFO("Closing RTAB-Map...");
+	delete rtabmap;
 	delete mainWindow;
-
 	delete app;
-
-	ULOGGER_INFO("All done!");
+	UINFO("All done!");
 
     return 0;
 }

bin/.gitignore

@@ -1,18 +1,5 @@
-/librtabmap_core.so
-/librtabmap_gui.so
-/librtabmap_utilite.so
-/rtabmap
-/rtabmap-bow_mapping
-/rtabmap-calibration
-/rtabmap-console
-/rtabmap-databaseViewer
-/rtabmap-dataRecorder
-/rtabmap-epipolar_geometry
-/rtabmap-extractObject
-/rtabmap-imagesJoiner
-/rtabmap-odometryViewer
-/rtabmap-rgbd_camera
-/rtabmap-rgbd_mapping
-/rtabmap-vocabularyComparison
-/uresourcegenerator
-/uresourcegenerator-0.3.0
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
+!data

build/.gitignore

@@ -1,17 +1,4 @@
-/app
-/CMakeFiles
-/corelib
-/examples
-/guilib
-/tools
-/utilite
-/cmake_install.cmake
-/cmake_uninstall.cmake
-/CMakeCache.txt
-/CPackConfig.cmake
-/CPackSourceConfig.cmake
-/install_manifest.txt
-/Makefile
-/RTABMapConfig.cmake
-/rtabmapConfigVersion.cmake
-/RTABMapConfigVersion.cmake
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore

cmake_modules/FindDC1394.cmake

@@ -0,0 +1,33 @@
+# - Find DC1394 alias libdc1394
+# This module finds an installed DC1394 package.
+#
+# It sets the following variables:
+#  DC1394_FOUND       - Set to false, or undefined, if DC1394 isn't found.
+#  DC1394_INCLUDE_DIRS - The DC1394 include directory.
+#  DC1394_LIBRARIES     - The DC1394 library to link against.
+
+find_path(DC1394_INCLUDE_DIRS NAMES dc1394.h PATH_SUFFIXES dc1394)
+find_library(DC1394_LIBRARIES NAMES dc1394)
+
+IF (DC1394_INCLUDE_DIRS AND DC1394_LIBRARIES)
+   SET(DC1394_FOUND TRUE)
+
+   #On Mac OS X
+   if(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+     set(DC1394_LIBRARIES ${DC1394_LIBRARIES} "-framework CoreServices")
+   endif(CMAKE_SYSTEM_NAME MATCHES "Darwin")
+
+ENDIF (DC1394_INCLUDE_DIRS AND DC1394_LIBRARIES)
+
+IF (DC1394_FOUND)
+   # show which DC1394 was found only if not quiet
+   IF (NOT DC1394_FIND_QUIETLY)
+      MESSAGE(STATUS "Found DC1394: ${DC1394_LIBRARIES}")
+   ENDIF (NOT DC1394_FIND_QUIETLY)
+ELSE (DC1394_FOUND)
+   # fatal error if DC1394 is required but not found
+   IF (DC1394_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find DC1394 (libdc1394)")
+   ENDIF (DC1394_FIND_REQUIRED)
+ENDIF (DC1394_FOUND)
+

cmake_modules/FindFLANN.cmake

@@ -0,0 +1,68 @@
+###############################################################################
+# Find FLANN
+#
+# This sets the following variables:
+# FLANN_FOUND - True if FLANN was found.
+# FLANN_INCLUDE_DIRS - Directories containing the FLANN include files.
+# FLANN_LIBRARIES - Libraries needed to use FLANN.
+# FLANN_DEFINITIONS - Compiler flags for FLANN.
+# If FLANN_USE_STATIC is specified and then look for static libraries ONLY else
+# look for shared ones
+#
+# Original from https://github.com/PointCloudLibrary/pcl/blob/master/cmake/Modules/FindFLANN.cmake
+#
+
+if(FLANN_USE_STATIC)
+  set(FLANN_RELEASE_NAME flann_cpp_s)
+  set(FLANN_DEBUG_NAME flann_cpp_s-gd)
+else(FLANN_USE_STATIC)
+  set(FLANN_RELEASE_NAME flann_cpp)
+  set(FLANN_DEBUG_NAME flann_cpp-gd)
+endif(FLANN_USE_STATIC)
+
+find_package(PkgConfig QUIET)
+if (FLANN_FIND_VERSION)
+    pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION})
+else(FLANN_FIND_VERSION)
+    pkg_check_modules(PC_FLANN flann)
+endif(FLANN_FIND_VERSION)
+
+set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER})
+
+find_path(FLANN_INCLUDE_DIR flann/flann.hpp
+          HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+          PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+          PATH_SUFFIXES include)
+
+find_library(FLANN_LIBRARY
+             NAMES ${FLANN_RELEASE_NAME}
+             HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+             PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+	     PATH_SUFFIXES lib)
+
+find_library(FLANN_LIBRARY_DEBUG 
+             NAMES ${FLANN_DEBUG_NAME} ${FLANN_RELEASE_NAME}
+	     HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}"
+	     PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" 
+	     PATH_SUFFIXES lib)
+
+if(NOT FLANN_LIBRARY_DEBUG)
+  set(FLANN_LIBRARY_DEBUG ${FLANN_LIBRARY})
+endif(NOT FLANN_LIBRARY_DEBUG)
+
+set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR})
+set(FLANN_LIBRARIES optimized ${FLANN_LIBRARY} debug ${FLANN_LIBRARY_DEBUG})
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARY FLANN_INCLUDE_DIR)
+
+mark_as_advanced(FLANN_LIBRARY FLANN_LIBRARY_DEBUG FLANN_INCLUDE_DIR)
+
+if(FLANN_FOUND)
+  IF (NOT FLANN_FIND_QUIETLY)
+    message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})")
+  ENDIF (NOT FLANN_FIND_QUIETLY)
+  if(FLANN_USE_STATIC)
+    add_definitions(-DFLANN_STATIC)
+  endif(FLANN_USE_STATIC)
+endif(FLANN_FOUND)

cmake_modules/FindFlyCapture2.cmake

@@ -0,0 +1,48 @@
+# - Find FlyCapture2 
+# This module finds an installed FlyCapture2+Triclops stereo camera package. (Point Grey SDK)
+#
+# It sets the following variables:
+#  FlyCapture2_FOUND       - Set to false, or undefined, if FlyCapture2 isn't found.
+#  FlyCapture2_INCLUDE_DIRS - The FlyCapture2 include directory.
+#  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
+
+if(CMAKE_CL_64)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64)
+else()
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib)
+endif()
+
+if(CMAKE_CL_64)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64)
+else()
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib)
+endif()
+
+#FlyCapture2 SDK
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include)
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
+
+# Triclops SDK
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include)
+find_library(Triclops_LIBRARY NAMES triclops triclops_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+   SET(FlyCapture2_FOUND TRUE)
+   SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+
+IF (FlyCapture2_FOUND)
+   # show which FlyCapture2 was found only if not quiet
+   IF (NOT FlyCapture2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_LIBRARIES}")
+   ENDIF (NOT FlyCapture2_FIND_QUIETLY)
+ELSE (FlyCapture2_FOUND)
+   # fatal error if FlyCapture2 is required but not found
+   IF (FlyCapture2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FlyCapture2 (FlyCapture2 Stereo Vision SDK)")
+   ENDIF (FlyCapture2_FIND_REQUIRED)
+ENDIF (FlyCapture2_FOUND)
+

cmake_modules/FindG2O.cmake

@@ -0,0 +1,82 @@
+#Pre-requisites: Look for csparse
+FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse )
+FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CSPARSE DEFAULT_MSG CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
+
+
+# G2O: Find the header files
+
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h)
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+  FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    )
+  
+  FIND_LIBRARY(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    )
+  
+  IF(NOT ${MYLIBRARY}_DEBUG)
+    IF(MYLIBRARY)
+      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+    ENDIF(MYLIBRARY)
+  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+  
+ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+  SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)
+  SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})
+  SET(G2O_LIBRARIES 
+	${G2O_STUFF_LIBRARY} 
+	${G2O_CORE_LIBRARY}
+	${G2O_SOLVER_CHOLMOD}
+	${G2O_SOLVER_CSPARSE} 
+	${G2O_SOLVER_CSPARSE_EXTENSION}
+	${G2O_TYPES_SLAM2D} 
+	${G2O_TYPES_SLAM3D} 
+	${CSPARSE_LIBRARY}
+        cholmod)
+  SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)

corelib/include/rtabmap/core/Camera.h

@@ -50,108 +50,40 @@ class RTABMAP_EXP Camera
 {
 public:
 	virtual ~Camera();
-	cv::Mat takeImage();
-	virtual bool init() = 0;
+	SensorData takeImage();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual bool isCalibrated() const = 0;
+	virtual std::string getSerial() const = 0;
+	int getNextSeqID() {return ++_seq;}
 
 	//getters
-	void getImageSize(unsigned int & width, unsigned int & height);
 	float getImageRate() const {return _imageRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
 
 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setImageSize(unsigned int width, unsigned int height);
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
 protected:
 	/**
 	 * Constructor
 	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
+	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 
-	virtual cv::Mat captureImage() = 0;
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureImage() = 0;
 
 private:
 	float _imageRate;
-	unsigned int _imageWidth;
-	unsigned int _imageHeight;
+	Transform _localTransform;
+	cv::Size _targetImageSize;
 	UTimer * _frameRateTimer;
-};
-
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-class RTABMAP_EXP CameraImages :
-	public Camera
-{
-public:
-	CameraImages(const std::string & path,
-			int startAt = 1,
-			bool refreshDir = false,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	virtual ~CameraImages();
-
-	virtual bool init();
-	std::string getPath() const {return _path;}
-
-protected:
-	virtual cv::Mat captureImage();
-
-private:
-	std::string _path;
-	int _startAt;
-	// If the list of files in the directory is refreshed
-	// on each call of takeImage()
-	bool _refreshDir;
-	int _count;
-	UDirectory * _dir;
-	std::string _lastFileName;
-};
-
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-class RTABMAP_EXP CameraVideo :
-	public Camera
-{
-public:
-	enum Source{kVideoFile, kUsbDevice};
-
-public:
-	CameraVideo(int usbDevice = 0,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	CameraVideo(const std::string & filePath,
-			float imageRate = 0,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	virtual ~CameraVideo();
-
-	virtual bool init();
-	int getUsbDevice() const {return _usbDevice;}
-	const std::string & getFilePath() const {return _filePath;}
-
-protected:
-	virtual cv::Mat captureImage();
-
-private:
-	// File type
-	std::string _filePath;
-
-	cv::VideoCapture _capture;
-	Source _src;
-
-	// Usb camera
-	int _usbDevice;
+	int _seq;
 };
 
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -38,40 +38,40 @@ class CameraEvent :
 {
 public:
 	enum Code {
-		kCodeImage,
-		kCodeImageDepth,
+		kCodeData,
 		kCodeNoMoreImages
 	};
 
 public:
-	CameraEvent(const cv::Mat & image, int seq=0) :
-		UEvent(kCodeImage),
-		data_(image, seq)
+	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
+		UEvent(kCodeData),
+		data_(image, seq, stamp),
+		cameraName_(cameraName)
 	{
 	}
+
 	CameraEvent() :
 		UEvent(kCodeNoMoreImages)
 	{
 	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
-		UEvent(kCodeImageDepth),
-		data_(image, depth, fx, fy, cx, cy, Transform(), localTransform, seq)
-	{
-	}
-	CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float fx, float fy, float cx, float cy, const Transform & localTransform, int seq=0) :
-		UEvent(kCodeImageDepth),
-		data_(image, depth, depth2d, fx, fy, cx, cy, Transform(), localTransform, seq)
+
+	CameraEvent(const SensorData & data, const std::string & cameraName = "") :
+		UEvent(kCodeData),
+		data_(data),
+		cameraName_(cameraName)
 	{
 	}
 
 	// Image or descriptors
 	const SensorData & data() const {return data_;}
+	const std::string & cameraName() const {return cameraName_;}
 
 	virtual ~CameraEvent() {}
 	virtual std::string getClassName() const {return std::string("CameraEvent");}
 
 private:
 	SensorData data_;
+	std::string cameraName_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

@@ -0,0 +1,213 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CAMERAMODEL_H_
+#define CAMERAMODEL_H_
+
+#include <opencv2/opencv.hpp>
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/Transform.h"
+
+namespace rtabmap {
+
+class RTABMAP_EXP CameraModel
+{
+public:
+	CameraModel();
+	// K is the camera intrinsic 3x3 CV_64FC1
+	// D is the distortion coefficients 1x5 CV_64FC1
+	// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
+	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
+	CameraModel(
+			const std::string & name,
+			const cv::Size & imageSize,
+			const cv::Mat & K,
+			const cv::Mat & D,
+			const cv::Mat & R,
+			const cv::Mat & P,
+			const Transform & localTransform = Transform::getIdentity());
+
+	// minimal
+	CameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = Transform::getIdentity(),
+			double Tx = 0.0f);
+	// minimal to be saved
+	CameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = Transform::getIdentity(),
+			double Tx = 0.0f);
+
+	virtual ~CameraModel() {}
+
+	bool isValid() const {return !K_.empty() &&
+									!D_.empty() &&
+									!R_.empty() &&
+									!P_.empty() &&
+									fx()>0.0 &&
+									fy()>0.0;}
+
+	void setName(const std::string & name) {name_=name;}
+	const std::string & name() const {return name_;}
+
+	double fx() const {return P_.at<double>(0,0);}
+	double fy() const {return P_.at<double>(1,1);}
+	double cx() const {return P_.at<double>(0,2);}
+	double cy() const {return P_.at<double>(1,2);}
+	double Tx() const {return P_.at<double>(0,3);}
+
+	const cv::Mat & K() const {return K_;} //intrinsic camera matrix
+	const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix
+	const cv::Mat & R() const {return R_;} //rectification matrix
+	const cv::Mat & P() const {return P_;} //projection matrix
+
+	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
+	const Transform & localTransform() const {return localTransform_;}
+
+	void setImageSize(const cv::Size & size) {imageSize_ = size;}
+	const cv::Size & imageSize() const {return imageSize_;}
+	int imageWidth() const {return imageSize_.width;}
+	int imageHeight() const {return imageSize_.height;}
+
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory) const;
+
+	void scale(double scale);
+
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+
+	// For depth images, your should use cv::INTER_NEAREST
+	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
+	cv::Mat rectifyDepth(const cv::Mat & raw) const;
+
+private:
+	std::string name_;
+	cv::Size imageSize_;
+	cv::Mat K_;
+	cv::Mat D_;
+	cv::Mat R_;
+	cv::Mat P_;
+	cv::Mat mapX_;
+	cv::Mat mapY_;
+	Transform localTransform_;
+};
+
+class RTABMAP_EXP StereoCameraModel
+{
+public:
+	StereoCameraModel() {}
+	StereoCameraModel(
+			const std::string & name,
+			const cv::Size & imageSize1,
+			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
+			const cv::Size & imageSize2,
+			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
+			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
+		right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
+		name_(name),
+		R_(R),
+		T_(T),
+		E_(E),
+		F_(F)
+	{
+	}
+	//minimal
+	StereoCameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(fx, fy, cx, cy, localTransform),
+		right_(fx, fy, cx, cy, localTransform, baseline*-fx)
+	{
+	}
+	//minimal to be saved
+	StereoCameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform::getIdentity()) :
+		left_(name+"_left", fx, fy, cx, cy, localTransform),
+		right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+		name_(name)
+	{
+	}
+	virtual ~StereoCameraModel() {}
+
+	bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
+
+	void setName(const std::string & name);
+	const std::string & name() const {return name_;}
+
+	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
+	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+
+	double baseline() const {return -right_.Tx()/right_.fx();}
+
+	const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
+	const cv::Mat & T() const {return T_;} //extrinsic translation matrix
+	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
+	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
+
+	void scale(double scale);
+
+	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
+	const Transform & localTransform() const {return left_.localTransform();}
+	Transform stereoTransform() const;
+
+	const CameraModel & left() const {return left_;}
+	const CameraModel & right() const {return right_;}
+
+private:
+	CameraModel left_;
+	CameraModel right_;
+	std::string name_;
+	cv::Mat R_;
+	cv::Mat T_;
+	cv::Mat E_;
+	cv::Mat F_;
+};
+
+
+} /* namespace rtabmap */
+#endif /* CAMERAMODEL_H_ */

corelib/include/rtabmap/core/CameraRGB.h

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include "rtabmap/core/Camera.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+class RTABMAP_EXP CameraImages :
+	public Camera
+{
+public:
+	CameraImages(const std::string & path,
+			int startAt = 1,
+			bool refreshDir = false,
+			bool rectifyImages = false,
+			bool isDepth = false,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	std::string getPath() const {return _path;}
+	unsigned int imagesCount() const;
+	std::vector<std::string> filenames() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	std::string _path;
+	int _startAt;
+	// If the list of files in the directory is refreshed
+	// on each call of takeImage()
+	bool _refreshDir;
+	bool _rectifyImages;
+	bool _isDepth;
+	int _count;
+	UDirectory * _dir;
+	std::string _lastFileName;
+
+	std::string _cameraName;
+	CameraModel _model;
+};
+
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+class RTABMAP_EXP CameraVideo :
+	public Camera
+{
+public:
+	enum Source{kVideoFile, kUsbDevice};
+
+public:
+	CameraVideo(int usbDevice = 0,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraVideo(const std::string & filePath,
+			bool rectifyImages = false,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraVideo();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	int getUsbDevice() const {return _usbDevice;}
+	const std::string & getFilePath() const {return _filePath;}
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	// File type
+	std::string _filePath;
+	bool _rectifyImages;
+
+	cv::VideoCapture _capture;
+	Source _src;
+
+	// Usb camera
+	int _usbDevice;
+	std::string _guid;
+
+	CameraModel _model;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraRGBD.h

@@ -29,23 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/SensorData.h"
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/USemaphore.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
 
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 
 #include <boost/signals2/connection.hpp>
 
-class UDirectory;
-class UTimer;
-
 namespace openni
 {
 class Device;
@@ -54,7 +47,16 @@ class VideoStream;
 
 namespace pcl
 {
-	class Grabber;
+class Grabber;
+}
+
+namespace libfreenect2
+{
+class Freenect2;
+class Freenect2Device;
+class SyncMultiFrameListener;
+class Registration;
+class PacketPipeline;
 }
 
 typedef struct _freenect_context freenect_context;
@@ -63,87 +65,34 @@ typedef struct _freenect_device freenect_device;
 namespace rtabmap
 {
 
-/**
- * Class CameraRGBD
- *
- */
-class RTABMAP_EXP CameraRGBD
-{
-public:
-	virtual ~CameraRGBD();
-	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
-	virtual bool init() = 0;
-
-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-	float getFx() const {return _fx;}
-	float getFy() const {return _fy;}
-	float getCx() const {return _cx;}
-	float getCy() const {return _cy;}
-
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setFx(float fx) {_fx = fx;}
-	void setFy(float fy) {_fy = fy;}
-	void setCx(float cx) {_cx = cx;}
-	void setCy(float cy) {_cy = cy;}
-
-protected:
-	/**
-	 * Constructor
-	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
-	 */
-	CameraRGBD(float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity(),
-				float fx = 0.0f,
-				float fy = 0.0f,
-				float cx = 0.0f,
-				float cy = 0.0f);
-
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;
-
-private:
-	float _imageRate;
-	Transform _localTransform;
-	UTimer * _frameRateTimer;
-	float _fx;
-	float _fy;
-	float _cx;
-	float _cy;
-};
-
 /////////////////////////
 // CameraOpenNIPCL
 /////////////////////////
 class RTABMAP_EXP CameraOpenni :
-	public CameraRGBD
+	public Camera
 {
 public:
-	static bool available() {return true;}
+	static bool available();
 
 public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity(),
-			float fx = 0.0f,
-			float fy = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f);
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
-
+#ifdef HAVE_OPENNI
     void image_cb (
     		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
+#endif
 
-    bool init();
+    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+    virtual bool isCalibrated() const;
+    virtual std::string getSerial() const;
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+	virtual SensorData captureImage();
 
 private:
     pcl::Grabber* interface_;
@@ -160,7 +109,7 @@ private:
 // CameraOpenNICV
 /////////////////////////
 class RTABMAP_EXP CameraOpenNICV :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -169,17 +118,15 @@ public:
 public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity(),
-					float fx = 0.0f,
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNICV();
 
-	virtual bool init();
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const {return "";} // unknown with OpenCV
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+	virtual SensorData captureImage();
 
 private:
 	bool _asus;
@@ -191,7 +138,7 @@ private:
 // CameraOpenNI2
 /////////////////////////
 class RTABMAP_EXP CameraOpenNI2 :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -199,23 +146,24 @@ public:
 	static bool exposureGainAvailable();
 
 public:
-	CameraOpenNI2(float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity(),
-					float fx = 0.0f,
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
+	CameraOpenNI2(const std::string & deviceId = "",
+					float imageRate = 0,
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
 
-	virtual bool init();
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
 
 	bool setAutoWhiteBalance(bool enabled);
 	bool setAutoExposure(bool enabled);
 	bool setExposure(int value);
 	bool setGain(int value);
+	bool setMirroring(bool enabled);
+	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+	virtual SensorData captureImage();
 
 private:
 	openni::Device * _device;
@@ -223,6 +171,8 @@ private:
 	openni::VideoStream * _depth;
 	float _depthFx;
 	float _depthFy;
+	std::string _deviceId;
+	bool _openNI2StampsAndIDsUsed;
 };
 
 
@@ -232,7 +182,7 @@ private:
 class FreenectDevice;
 
 class RTABMAP_EXP CameraFreenect :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available();
@@ -241,17 +191,15 @@ public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
-					float fx = 0.0f,
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
+					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
 
-	bool init();
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
 
 protected:
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+	virtual SensorData captureImage();
 
 private:
 	int deviceId_;
@@ -259,4 +207,87 @@ private:
 	FreenectDevice * freenectDevice_;
 };
 
+/////////////////////////
+// CameraFreenect2
+/////////////////////////
+
+class RTABMAP_EXP CameraFreenect2 :
+	public Camera
+{
+public:
+	static bool available();
+
+	enum Type{
+		kTypeRGBDepthSD,
+		kTypeRGBDepthHD,
+		kTypeIRDepth,
+		kTypeRGBIR
+	};
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect2(int deviceId= 0,
+					Type type = kTypeRGBDepthSD,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraFreenect2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	int deviceId_;
+	Type type_;
+	StereoCameraModel stereoModel_;
+	libfreenect2::Freenect2 * freenect2_;
+	libfreenect2::Freenect2Device *dev_;
+	libfreenect2::PacketPipeline * pipeline_;
+	libfreenect2::SyncMultiFrameListener * listener_;
+	libfreenect2::Registration * reg_;
+};
+
+
+/////////////////////////
+// CameraRGBDImages
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraRGBDImages :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraRGBDImages(
+			const std::string & pathRGBImages,
+			const std::string & pathDepthImages,
+			double depthScaleFactor = 1.0,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRGBDImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	CameraImages * cameraRGB_;
+	CameraImages * cameraDepth_;
+	double depthScaleFactor_;
+	bool filenamesAreTimestamps_;
+	std::string timestampsPath_;
+	std::list<double> stamps_;
+	CameraModel cameraModel_;
+	std::string cameraName_;
+};
+
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -0,0 +1,176 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include <list>
+
+namespace FlyCapture2
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraStereoDC1394
+/////////////////////////
+class DC1394Device;
+
+class RTABMAP_EXP CameraStereoDC1394 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoDC1394();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	DC1394Device *device_;
+	StereoCameraModel stereoModel_;
+};
+
+/////////////////////////
+// CameraStereoFlyCapture2
+/////////////////////////
+class RTABMAP_EXP CameraStereoFlyCapture2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoFlyCapture2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	FlyCapture2::Camera * camera_;
+	void * triclopsCtx_; // TriclopsContext
+};
+
+/////////////////////////
+// CameraStereoImages
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraStereoImages :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoImages(
+			const std::string & pathLeftImages,
+			const std::string & pathRightImages,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoImages(
+			const std::string & pathLeftRightImages,
+			bool filenamesAreTimestamps = false,
+			const std::string & timestampsPath = "", // "times.txt"
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	CameraImages * camera_;
+	CameraImages * camera2_;
+	bool filenamesAreTimestamps_;
+	std::string timestampsPath_;
+	bool rectifyImages_;
+	std::list<double> stamps_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+};
+
+
+/////////////////////////
+// CameraStereoVideo
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraStereoVideo :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoVideo(
+			const std::string & path,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoVideo();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	cv::VideoCapture capture_;
+	std::string path_;
+	bool rectifyImages_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -36,7 +36,6 @@ namespace rtabmap
 {
 
 class Camera;
-class CameraRGBD;
 
 /**
  * Class CameraThread
@@ -49,10 +48,11 @@ class RTABMAP_EXP CameraThread :
 public:
 	// ownership transferred
 	CameraThread(Camera * camera);
-	CameraThread(CameraRGBD * camera);
 	virtual ~CameraThread();
 
-	bool init(); // call camera->init()
+	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -60,15 +60,16 @@ public:
 	void setImageRate(float imageRate);
 
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
-	CameraRGBD * cameraRGBD() {return _cameraRGBD;} // return null if not set, valid until CameraThread is deleted
 
 private:
 	virtual void mainLoop();
+	virtual void mainLoopKill();
 
 private:
 	Camera * _camera;
-	CameraRGBD * _cameraRGBD;
-	int _seq;
+	bool _mirroring;
+	bool _colorOnly;
+	bool _stereoToDepth;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Compression.h

@@ -0,0 +1,87 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef COMPRESSION_H_
+#define COMPRESSION_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UThread.h>
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+/**
+ * Compress image or data
+ *
+ * Example compression:
+ *   cv::Mat image;// an image
+ *   CompressionThread ct(image);
+ *   ct.start();
+ *   ct.join();
+ *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ *
+ * Example uncompression
+ *   std::vector<unsigned char> bytes;// a compressed image
+ *   CompressionThread ct(bytes);
+ *   ct.start();
+ *   ct.join();
+ *   cv::Mat image = ct.getUncompressedData();
+ */
+class RTABMAP_EXP CompressionThread : public UThread
+{
+public:
+	// format : ".png" ".jpg" "" (empty is general)
+	CompressionThread(const cv::Mat & mat, const std::string & format = "");
+	CompressionThread(const cv::Mat & bytes, bool isImage);
+	const cv::Mat & getCompressedData() const {return compressedData_;}
+	cv::Mat & getUncompressedData() {return uncompressedData_;}
+protected:
+	virtual void mainLoop();
+private:
+	cv::Mat compressedData_;
+	cv::Mat uncompressedData_;
+	std::string format_;
+	bool image_;
+	bool compressMode_;
+};
+
+std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
+
+cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+
+std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
+cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
+
+cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
+cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
+
+} /* namespace rtabmap */
+#endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -38,13 +38,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UMutex.h"
 #include "rtabmap/utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
 
 namespace rtabmap {
 
 class Signature;
-class SMSignature;
 class VWDictionary;
 class VisualWord;
 
@@ -59,6 +60,9 @@ class VisualWord;
 //
 class RTABMAP_EXP DBDriver : public UThreadNode
 {
+public:
+	static DBDriver * create(const ParametersMap & parameters = ParametersMap());
+
 public:
 	virtual ~DBDriver();
 
@@ -72,19 +76,28 @@ public:
 	void asyncSave(VisualWord * vw); //ownership transferred
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
+
+	// Warning: the following functions don't look in the trash, direct database modifications
+	void generateGraph(
+			const std::string & fileName,
+			const std::set<int> & ids = std::set<int>(),
+			const std::map<int, Signature *> & otherSignatures = std::map<int, Signature *>());
+	void addLink(const Link & link);
+	void removeLink(int from, int to);
+	void updateLink(const Link & link);
 
 public:
 	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
-	bool getSignature(int signatureId, Signature ** s);
-	bool getVisualWord(int wordId, VisualWord ** vw);
 
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection();
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
+	std::string getDatabaseVersion() const;
 
 	void executeNoResult(const std::string & sql) const;
 
@@ -95,17 +108,18 @@ public:
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
 
 	// Specific queries...
-	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
-	void getPose(int signatureId, Transform & pose, int & mapId) const;
-	void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
-	void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
+	void loadNodeData(std::list<Signature *> & signatures) const;
+	void getNodeData(int signatureId, SensorData & data) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
+	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
 	void getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodeIdByLabel(const std::string & label, int & id) const;
+	void getAllLabels(std::map<int, std::string> & labels) const;
 
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -115,6 +129,7 @@ private:
 	virtual void disconnectDatabaseQuery() = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
+	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 
@@ -122,25 +137,27 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
 
+	virtual void addLinkQuery(const Link & link) const = 0;
+	virtual void updateLinkQuery(const Link & link) const = 0;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
-	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const = 0;
-	virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
+	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
-	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 
 private:
 	//non-abstract methods
@@ -159,6 +176,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	bool _timestampUpdate;
 };
 
 }

corelib/include/rtabmap/core/DBReader.h

@@ -34,10 +34,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/OdometryEvent.h>
 
 #include <opencv2/core/core.hpp>
 
 #include <set>
+#include <list>
 
 namespace rtabmap {
 
@@ -48,34 +50,35 @@ public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 float delayToStartSec = 0.0f);
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
+	DBReader(const std::list<std::string> & databasePaths,
+			 float frameRate = 0.0f,
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	void getNextImage(cv::Mat & image,
-			cv::Mat & depth,
-			cv::Mat & depth2d,
-			float & fx, float & fy,
-			float & cx, float & cy,
-			Transform & localTransform,
-			Transform & pose,
-			int & seq);
+	OdometryEvent getNextData();
 
 protected:
 	virtual void mainLoopBegin();
 	virtual void mainLoop();
 
 private:
-	std::string _path;
-	float _frameRate;
+	std::list<std::string> _paths;
+	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
-	float _delayToStartSec;
+	bool _ignoreGoalDelay;
+	bool _goalsIgnored;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	double _previousStamp;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Features2d.h

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef KEYPOINTDESCRIPTOR_H_
-#define KEYPOINTDESCRIPTOR_H_
+#ifndef FEATURES2D_H_
+#define FEATURES2D_H_
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <list>
 #include "rtabmap/core/Parameters.h"
 
+#if CV_MAJOR_VERSION < 3
 namespace cv{
 class SURF;
 class SIFT;
@@ -45,6 +46,44 @@ namespace gpu {
 	class FAST_GPU;
 }
 }
+typedef cv::SIFT CV_SIFT;
+typedef cv::SURF CV_SURF;
+typedef cv::ORB CV_ORB;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::FREAK CV_FREAK;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::BriefDescriptorExtractor CV_BRIEF;
+typedef cv::BRISK CV_BRISK;
+typedef cv::gpu::SURF_GPU CV_SURF_GPU;
+typedef cv::gpu::ORB_GPU CV_ORB_GPU;
+typedef cv::gpu::FAST_GPU CV_FAST_GPU;
+#else
+namespace cv{
+namespace xfeatures2d {
+class FREAK;
+class BriefDescriptorExtractor;
+class SIFT;
+class SURF;
+}
+namespace cuda {
+class FastFeatureDetector;
+class ORB;
+class SURF_CUDA;
+}
+}
+typedef cv::xfeatures2d::SIFT CV_SIFT;
+typedef cv::xfeatures2d::SURF CV_SURF;
+typedef cv::ORB CV_ORB;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
+typedef cv::BRISK CV_BRISK;
+typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
+typedef cv::cuda::ORB CV_ORB_GPU;
+typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
+#endif
+
 
 namespace rtabmap {
 
@@ -89,21 +128,26 @@ public:
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
+	int getMaxFeatures() const {return maxFeatures_;}
+
 public:
 	virtual ~Feature2D() {}
 
-	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
 	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
-	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual void parseParameters(const ParametersMap & parameters);
 	virtual Feature2D::Type getType() const = 0;
 
 protected:
-	Feature2D(const ParametersMap & parameters = ParametersMap()) {}
+	Feature2D(const ParametersMap & parameters = ParametersMap());
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+
+private:
+	int maxFeatures_;
 };
 
 //SURF
@@ -129,8 +173,8 @@ private:
 	float gpuKeypointsRatio_;
 	bool gpuVersion_;
 
-	cv::SURF * _surf;
-	cv::gpu::SURF_GPU * _gpuSurf;
+	cv::Ptr<CV_SURF> _surf;
+	cv::Ptr<CV_SURF_GPU> _gpuSurf;
 };
 
 //SIFT
@@ -154,7 +198,7 @@ private:
 	double edgeThreshold_;
 	double sigma_;
 
-	cv::SIFT * _sift;
+	cv::Ptr<CV_SIFT> _sift;
 };
 
 //ORB
@@ -185,8 +229,8 @@ private:
 	int fastThreshold_;
 	bool nonmaxSuppresion_;
 
-	cv::ORB * _orb;
-	cv::gpu::ORB_GPU * _gpuOrb;
+	cv::Ptr<CV_ORB> _orb;
+	cv::Ptr<CV_ORB_GPU> _gpuOrb;
 };
 
 //FAST
@@ -207,8 +251,8 @@ private:
 	bool gpu_;
 	double gpuKeypointsRatio_;
 
-	cv::FastFeatureDetector * _fast;
-	cv::gpu::FAST_GPU * _gpuFast;
+	cv::Ptr<CV_FAST> _fast;
+	cv::Ptr<CV_FAST_GPU> _gpuFast;
 };
 
 //FAST_BRIEF
@@ -227,7 +271,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //FAST_FREAK
@@ -249,7 +293,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //GFTT
@@ -272,7 +316,7 @@ private:
 	bool _useHarrisDetector;
 	double _k;
 
-	cv::GFTTDetector * _gftt;
+	cv::Ptr<CV_GFTT> _gftt;
 };
 
 //GFTT_BRIEF
@@ -291,7 +335,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //GFTT_FREAK
@@ -313,7 +357,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //BRISK
@@ -335,10 +379,10 @@ private:
 	int octaves_;
 	float patternScale_;
 
-	cv::BRISK * brisk_;
+	cv::Ptr<CV_BRISK> brisk_;
 };
 
 
 }
 
-#endif /* KEYPOINTDESCRIPTOR_H_ */
+#endif /* FEATURES2D_H_ */

corelib/include/rtabmap/core/Graph.h

@@ -0,0 +1,355 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GRAPH_H_
+#define GRAPH_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <map>
+#include <list>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+class Memory;
+
+namespace graph {
+
+////////////////////////////////////////////
+// Graph optimizers
+////////////////////////////////////////////
+class RTABMAP_EXP Optimizer
+{
+public:
+	enum Type {
+		kTypeUndef = -1,
+		kTypeTORO = 0,
+		kTypeG2O = 1,
+		kTypeGTSAM = 2,
+		kTypeCVSBA = 3
+	};
+	static Optimizer * create(const ParametersMap & parameters);
+	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
+
+	// Get connected poses and constraints from a set of links
+	static void getConnectedGraph(
+			int fromId,
+			const std::map<int, Transform> & posesIn,
+			const std::multimap<int, Link> & linksIn, // only one link between two poses
+			std::map<int, Transform> & posesOut,
+			std::multimap<int, Link> & linksOut,
+			int depth = 0);
+
+public:
+	virtual ~Optimizer() {}
+
+	virtual Type type() const = 0;
+
+	int iterations() const {return iterations_;}
+	bool isSlam2d() const {return slam2d_;}
+	bool isCovarianceIgnored() const {return covarianceIgnored_;}
+	double epsilon() const {return epsilon_;}
+	bool isRobust() const {return robust_;}
+
+	// inherited classes should implement one of these methods
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+protected:
+	Optimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust());
+	Optimizer(const ParametersMap & parameters);
+
+private:
+	int iterations_;
+	bool slam2d_;
+	bool covarianceIgnored_;
+	double epsilon_;
+	bool robust_;
+};
+
+class RTABMAP_EXP TOROOptimizer : public Optimizer
+{
+public:
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints);
+	static bool loadGraph(
+			const std::string & fileName,
+			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & edgeConstraints);
+
+public:
+	TOROOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
+	TOROOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~TOROOptimizer() {}
+
+	virtual Type type() const {return kTypeTORO;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+};
+
+class RTABMAP_EXP G2OOptimizer : public Optimizer
+{
+public:
+	static bool available();
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			bool useRobustConstraints = false);
+
+public:
+	G2OOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	G2OOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~G2OOptimizer() {}
+
+	virtual Type type() const {return kTypeG2O;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+};
+
+class RTABMAP_EXP GTSAMOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	GTSAMOptimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
+			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	GTSAMOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~GTSAMOptimizer() {}
+
+	virtual Type type() const {return kTypeGTSAM;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0);
+};
+
+class RTABMAP_EXP CVSBAOptimizer : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
+		Optimizer(iterations, slam2d, covarianceIgnored),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	CVSBAOptimizer(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	virtual ~CVSBAOptimizer() {}
+
+	virtual Type type() const {return kTypeCVSBA;}
+
+	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
+	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
+
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
+private:
+	float inlierDistance_;
+	float minInliers_;
+};
+
+bool RTABMAP_EXP exportPoses(
+		const std::string & filePath,
+		int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
+		const std::map<int, double> & stamps); // required for format 1
+
+////////////////////////////////////////////
+// Graph utilities
+////////////////////////////////////////////
+std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to);
+std::multimap<int, int>::iterator RTABMAP_EXP findLink(
+		std::multimap<int, int> & links,
+		int from,
+		int to);
+std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
+		const std::multimap<int, Link> & links,
+		int from,
+		int to);
+std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
+		const std::multimap<int, int> & links,
+		int from,
+		int to);
+
+/**
+ * Get only the the most recent or older poses in the defined radius.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
+ * @return A map containing only most recent or older poses in the the defined radius
+ */
+std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		bool keepLatest = true);
+
+/**
+ * Get all neighbor nodes in a fixed radius around each pose.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @return A map between each pose id and its neighbors found in the radius
+ */
+std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle);
+
+/**
+ * Perform A* path planning in the graph.
+ * @param poses The graph's poses
+ * @param links The graph's links (from node id -> to node id)
+ * @param from initial node
+ * @param to final node
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @return the path ids from id "from" to id "to" including initial and final nodes.
+ */
+std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
+			const std::map<int, rtabmap::Transform> & poses,
+			const std::multimap<int, int> & links,
+			int from,
+			int to,
+			bool updateNewCosts = false);
+
+/**
+ * Perform Dijkstra path planning in the graph.
+ * @param fromId initial node
+ * @param toId final node
+ * @param memory The graph's memory
+ * @param lookInDatabase check links in database
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
+ */
+std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
+		int fromId,
+		int toId,
+		const Memory * memory,
+		bool lookInDatabase = true,
+		bool updateNewCosts = false,
+		float linearVelocity = 0.0f,   // m/sec
+		float angularVelocity = 0.0f); // rad/sec
+
+int RTABMAP_EXP findNearestNode(
+		const std::map<int, rtabmap::Transform> & nodes,
+		const rtabmap::Transform & targetPose);
+
+/**
+ * Get nodes near the query
+ * @param nodeId the query id
+ * @param nodes the nodes to search for
+ * @param maxNearestNeighbors Maximum nearest neighbor to get. 0 means all.
+ * @param radius radius to search for (m)
+ * @return the nodes with squared distance to query node.
+ */
+std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		float radius);
+std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		float radius);
+
+float RTABMAP_EXP computePathLength(
+		const std::vector<std::pair<int, Transform> > & path,
+		unsigned int fromIndex = 0,
+		unsigned int toIndex = 0);
+
+std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
+		std::map<int, Transform> poses,
+		const std::multimap<int, Link> & links);
+
+
+} /* namespace graph */
+
+} /* namespace rtabmap */
+#endif /* GRAPH_H_ */

corelib/include/rtabmap/core/Link.h

@@ -29,26 +29,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LINK_H_
 
 #include <rtabmap/core/Transform.h>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
 class Link
 {
 public:
-	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
-	Link() :
-		from_(0),
-		to_(0),
-		type_(kUndef)
-	{
-	}
-	Link(int from, int to, const Transform & transform, Type type) :
-		from_(from),
-		to_(to),
-		transform_(transform),
-		type_(type)
-	{
-	}
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
+	Link();
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
+			const cv::Mat & userData = cv::Mat());
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			double rotVariance,
+			double transVariance,
+			const cv::Mat & userData = cv::Mat());
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
 
@@ -56,12 +58,37 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
+	const cv::Mat & infMatrix() const {return infMatrix_;}
+	double rotVariance() const;
+	double transVariance() const;
+
+	void setFrom(int from) {from_ = from;}
+	void setTo(int to) {to_ = to;}
+	void setTransform(const Transform & transform) {transform_ = transform;}
+	void setType(Type type) {type_ = type;}
+	void setInfMatrix(const cv::Mat & infMatrix);
+	void setVariance(double rotVariance, double transVariance);
+
+	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
+	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
+	void uncompressUserData();
+	cv::Mat uncompressUserDataConst() const;
+
+	Link merge(const Link & link, Type outputType) const;
+	Link inverse() const;
 
 private:
 	int from_;
 	int to_;
 	Transform transform_;
 	Type type_;
+	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -42,12 +42,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/point_types.h>
 
 namespace rtabmap {
 
 class Signature;
 class DBDriver;
-class GraphNode;
 class VWDictionary;
 class VisualWord;
 class Feature2D;
@@ -65,7 +65,12 @@ public:
 	virtual ~Memory();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	bool update(const SensorData & data, Statistics * stats = 0);
+	bool update(const SensorData & data,
+			Statistics * stats = 0);
+	bool update(const SensorData & data,
+			const Transform & pose,
+			const cv::Mat & covariance,
+			Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
 			const ParametersMap & parameters = ParametersMap(),
@@ -73,48 +78,73 @@ public:
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId();
+	void updateAge(int signatureId);
 
 	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
-	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	int cleanup();
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, bool global);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform);
-	std::map<int, int> getNeighborsId(int signatureId,
-			int margin,
+	bool addLink(const Link & link);
+	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
+	void updateLink(int fromId, int toId, const Transform & transform, const cv::Mat & covariance);
+	void removeAllVirtualLinks();
+	void removeVirtualLinks(int signatureId);
+	std::map<int, int> getNeighborsId(
+			int signatureId,
+			int maxGraphDepth,
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
+			bool ignoreIntermediateNodes = false,
 			double * dbAccessTime = 0) const;
+	std::map<int, float> getNeighborsIdRadius(
+			int signatureId,
+			float radius,
+			const std::map<int, Transform> & optimizedPoses,
+			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
-	void rejectLoopClosure(int oldId, int newId);
+	void removeLink(int idA, int idB);
 
 	//getters
-	const std::set<int> & getWorkingMem() const {return _workingMem;}
+	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
-	void getPose(int locationId,
-			Transform & pose,
+	std::map<int, Link> getNeighborLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	std::map<int, Transform> getNeighborLinks(int signatureId,
-			bool ignoreNeighborByLoopClosure = false,
+	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
-	void getLoopClosureIds(int signatureId,
-			std::map<int, Transform> & loopClosureIds,
-			std::map<int, Transform> & childLoopClosureIds,
+	std::map<int, Link> getLinks(int signatureId,
 			bool lookInDatabase = false) const;
+	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true) const;
 	bool isRawDataKept() const {return _rawDataKept;}
+	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
 	const Signature * getLastWorkingSignature() const;
+	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
+	bool labelSignature(int id, const std::string & label);
+	std::map<int, std::string> getAllLabels() const;
+	bool setUserData(int id, const cv::Mat & data);
 	int getDatabaseMemoryUsed() const; // in bytes
+	std::string getDatabaseVersion() const;
 	double getDbSavingTime() const;
-	int getMapId(int signatureId) const;
+	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
+	bool getNodeInfo(int signatureId,
+			Transform & odomPose,
+			int & mapId,
+			int & weight,
+			std::string & label,
+			double & stamp,
+			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	Signature getSignatureData(int locationId, bool uncompressedData = false);
+	SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
+	void getNodeWords(int nodeId,
+			std::multimap<int, cv::KeyPoint> & words,
+			std::multimap<int, pcl::PointXYZ> & words3);
+	SensorData getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -123,8 +153,7 @@ public:
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
 	bool isIDsGenerated() const {return _generateIds;}
-	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
-	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
+	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
 	const Feature2D * getFeature2D() const {return _feature2D;}
 
 	void setRoi(const std::string & roi);
@@ -134,10 +163,7 @@ public:
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
 
-	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
-	void createGraph(GraphNode * parent,
-			unsigned int maxDepth,
-			const std::set<int> & endIds = std::set<int>());
+	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
 
 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;
@@ -145,30 +171,31 @@ public:
 
 	// RGB-D stuff
 	void getMetricConstraints(
-			const std::vector<int> & ids,
+			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
 	float getBowInlierDistance() const {return _bowInlierDistance;}
 	int getBowIterations() const {return _bowIterations;}
 	int getBowMinInliers() const {return _bowMinInliers;}
-	float getBowMaxDepth() const {return _bowMaxDepth;}
 	bool getBowForce2D() const {return _bowForce2D;}
-	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0) const;
-	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0) const;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0) const;
+	Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
 			const std::map<int, Transform> & poses,
-			std::string * rejectedMsg = 0);
+			std::string * rejectedMsg = 0,
+			int * inliers = 0,
+			double * variance = 0);
 
 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature);
+	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
-	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
+	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
 	void addSignatureToWm(Signature * signature);
 	Signature * _getSignature(int id) const;
@@ -184,7 +211,7 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
-			bool keepRawData=false,
+			const Transform & pose,
 			Statistics * stats = 0);
 
 	//keypoint stuff
@@ -200,26 +227,35 @@ private:
 	// parameters
 	float _similarityThreshold;
 	bool _rawDataKept;
-	bool _keepRehearsedNodesInDb;
+	bool _binDataKept;
+	bool _saveDepth16Format;
+	bool _notLinkedNodesKeptInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
 	float _recentWmRatio;
+	bool _transferSortingByWeightId;
 	bool _idUpdatedToNewOneRehearsal;
 	bool _generateIds;
 	bool _badSignaturesIgnored;
+	int _imageDecimation;
+	float _laserScanVoxelSize;
+	bool _localSpaceLinksKeptInWM;
+	float _rehearsalMaxDistance;
+	float _rehearsalMaxAngle;
+	bool _rehearsalWeightIgnoredWhileMoving;
 
 	int _idCount;
 	int _idMapCount;
 	Signature * _lastSignature;
-	int _lastGlobalLoopClosureParentId;
-	int _lastGlobalLoopClosureChildId;
-	bool _memoryChanged; // False by default, become true when Memory::update() is called.
+	int _lastGlobalLoopClosureId;
+	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
+	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	bool _postInitClosingEvents;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
-	std::set<int> _workingMem; // id,age
+	std::map<int, double> _workingMem; // id,age
 
 	//Keypoint stuff
 	VWDictionary * _vwd;
@@ -229,27 +265,32 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 	float _wordsMaxDepth; // 0=inf
-	int _wordsPerImageTarget; // <0=none, 0=inf
 	std::vector<float> _roiRatios; // size 4
 
 	// RGBD-SLAM stuff
 	int _bowMinInliers;
 	float _bowInlierDistance;
 	int _bowIterations;
-	float _bowMaxDepth;
+	int _bowRefineIterations;
 	bool _bowForce2D;
+	float _bowEpipolarGeometryVar;
+	int _bowEstimationType;
+	double _bowPnPReprojError;
+	int _bowPnPFlags;
+	bool _bowVarianceFromInliersCount;
+	float _icpMaxTranslation;
+	float _icpMaxRotation;
 	int _icpDecimation;
 	float _icpMaxDepth;
 	float _icpVoxelSize;
 	int _icpSamples;
 	float _icpMaxCorrespondenceDistance;
 	int _icpMaxIterations;
-	float _icpMaxFitness;
+	float _icpCorrespondenceRatio;
 	bool _icpPointToPlane;
 	int _icpPointToPlaneNormalNeighbors;
 	float _icp2MaxCorrespondenceDistance;
 	int _icp2MaxIterations;
-	float _icp2MaxFitness;
 	float _icp2CorrespondenceRatio;
 	float _icp2VoxelSize;
 

corelib/include/rtabmap/core/Odometry.h

@@ -30,19 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-#include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UEventsHandler.h>
-#include <rtabmap/utilite/UEvent.h>
-#include <rtabmap/utilite/UMutex.h>
-#include <rtabmap/utilite/USemaphore.h>
-
-#include <rtabmap/core/Parameters.h>
-
+#include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
-
-#include <opencv2/opencv.hpp>
-
-#include <pcl/common/eigen.h>
+#include <rtabmap/core/Parameters.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
@@ -51,47 +41,61 @@ class UTimer;
 namespace rtabmap {
 
 class Feature2D;
+class OdometryInfo;
+class ParticleFilter;
 
 class RTABMAP_EXP Odometry
 {
 public:
-	virtual ~Odometry() {}
-	Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	virtual ~Odometry();
+	Transform process(const SensorData & data, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
-	bool isLargeEnoughTransform(const Transform & transform);
-
 	//getters
 	const Transform & getPose() const {return _pose;}
-	int getMaxFeatures() const  {return _maxFeatures;}
 	const std::string & getRoiRatios() const {return _roiRatios;}
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getRefineIterations() const {return _refineIterations;}
-	float getFeaturesRatio() const {return _featuresRatio;}
 	float getMaxDepth() const {return _maxDepth;}
-	float geLinearUpdate() const {return _linearUpdate;}
-	float getAngularUpdate() const {return _angularUpdate;}
+	bool isInfoDataFilled() const {return _fillInfoData;}
+	int getEstimationType() const {return _estimationType;}
+	double getPnPReprojError() const {return _pnpReprojError;}
+	int  getPnPFlags() const {return _pnpFlags;}
+	const Transform & previousTransform() const {return previousTransform_;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
 
 private:
-	int _maxFeatures;
 	std::string _roiRatios;
 	int _minInliers;
 	float _inlierDistance;
 	int _iterations;
 	int _refineIterations;
-	float _featuresRatio;
 	float _maxDepth;
-	float _linearUpdate;
-	float _angularUpdate;
 	int _resetCountdown;
 	bool _force2D;
+	bool _holonomic;
+	bool _particleFiltering;
+	int _particleSize;
+	float _particleNoiseT;
+	float _particleLambdaT;
+	float _particleNoiseR;
+	float _particleLambdaR;
+	bool _fillInfoData;
+	int _estimationType;
+	double _pnpReprojError;
+	int _pnpFlags;
+	bool _varianceFromInliersCount;
 	Transform _pose;
 	int _resetCurrentCount;
+	double previousStamp_;
+	Transform previousTransform_;
+	float distanceTravelled_;
+
+	std::vector<ParticleFilter *> filters_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);
@@ -106,18 +110,19 @@ public:
 	virtual ~OdometryBOW();
 
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	const std::multimap<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
+	const std::map<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
 	const Memory * getMemory() const {return _memory;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
 
 private:
 	//Parameters
 	int _localHistoryMaxSize;
+	std::string _fixedLocalMapPath;
 
 	Memory * _memory;
-	std::multimap<int, pcl::PointXYZ> localMap_;
+	std::map<int, pcl::PointXYZ> localMap_;
 };
 
 class RTABMAP_EXP OdometryOpticalFlow : public Odometry
@@ -133,10 +138,8 @@ public:
 	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
-	Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
-	Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
-	Transform computeTransformMono(const SensorData & image, int * quality, int * features);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
+
 private:
 	//Parameters:
 	int flowWinSize_;
@@ -157,11 +160,48 @@ private:
 	Feature2D * feature2D_;
 
 	cv::Mat refFrame_;
-	cv::Mat refRightFrame_;
 	std::vector<cv::Point2f> refCorners_;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
 };
 
+class RTABMAP_EXP OdometryMono : public Odometry
+{
+public:
+	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryMono();
+	virtual void reset(const Transform & initialPose);
+
+private:
+	virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
+private:
+	//Parameters:
+	int flowWinSize_;
+	int flowIterations_;
+	double flowEps_;
+	int flowMaxLevel_;
+
+	int stereoWinSize_;
+	int stereoIterations_;
+	double stereoEps_;
+	int stereoMaxLevel_;
+	float stereoMaxSlope_;
+
+	Memory * memory_;
+	int localHistoryMaxSize_;
+	float initMinFlow_;
+	float initMinTranslation_;
+	float minTranslation_;
+	float fundMatrixReprojError_;
+	float fundMatrixConfidence_;
+
+	cv::Mat refDepthOrRight_;
+	std::map<int, cv::Point2f> cornersMap_;
+	std::map<int, cv::Point3f> localMap_;
+	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
+	std::map<int, Transform> keyFramePoses_;
+	float maxVariance_;
+};
+
 class RTABMAP_EXP OdometryICP : public Odometry
 {
 public:
@@ -170,13 +210,13 @@ public:
 			int samples = 0,
 			float maxCorrespondenceDistance = 0.05f,
 			int maxIterations = 30,
-			float maxFitness = 0.01f,
+			float correspondenceRatio = 0.7f,
 			bool pointToPlane = true,
 			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
+	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
 
 private:
 	int _decimation;
@@ -184,39 +224,12 @@ private:
 	float _samples;
 	float _maxCorrespondenceDistance;
 	int	_maxIterations;
-	float _maxFitness;
+	float _correspondenceRatio;
 	bool _pointToPlane;
 
 	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
 	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
 };
 
-class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
-public:
-	// take ownership of Odometry
-	OdometryThread(Odometry * odometry);
-	virtual ~OdometryThread();
-
-protected:
-	virtual void handleEvent(UEvent * event);
-
-private:
-	void mainLoopKill();
-
-	//============================================================
-	// MAIN LOOP
-	//============================================================
-	void mainLoop();
-	void addData(const SensorData & data);
-	void getData(SensorData & data);
-
-private:
-	USemaphore _dataAdded;
-	UMutex _dataMutex;
-	SensorData _dataBuffer;
-	Odometry * _odometry;
-	bool _resetOdometry;
-};
-
 } /* namespace rtabmap */
 #endif /* ODOMETRY_H_ */

corelib/include/rtabmap/core/OdometryEvent.h

@@ -29,37 +29,80 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYEVENT_H_
 
 #include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/OdometryInfo.h"
 
 namespace rtabmap {
 
 class OdometryEvent : public UEvent
 {
 public:
+	static cv::Mat generateCovarianceMatrix(float rotVariance, float transVariance)
+	{
+		UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+		UASSERT(uIsFinite(transVariance) && transVariance>0);
+		cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+		covariance.at<double>(0,0) = transVariance;
+		covariance.at<double>(1,1) = transVariance;
+		covariance.at<double>(2,2) = transVariance;
+		covariance.at<double>(3,3) = rotVariance;
+		covariance.at<double>(4,4) = rotVariance;
+		covariance.at<double>(5,5) = rotVariance;
+		return covariance;
+	}
+public:
+	OdometryEvent() :
+		_covariance(cv::Mat::eye(6,6,CV_64FC1))
+	{
+	}
 	OdometryEvent(
-		const SensorData & data, int quality = -1, float time = 0.0f, int features = 0, int localMapSize = 0) :
+		const SensorData & data,
+		const Transform & pose,
+		const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
+		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
-			_quality(quality),
-			_time(time),
-			_features(features),
-			_localMapSize(localMapSize)
-	{}
+			_pose(pose),
+			_info(info)
+	{
+		UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(1,1)) && covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(covariance.at<double>(5,5)) && covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		_covariance = covariance;
+	}
+	OdometryEvent(
+		const SensorData & data,
+		const Transform & pose,
+		double rotVariance = 1.0,
+		double transVariance = 1.0,
+		const OdometryInfo & info = OdometryInfo()) :
+			_data(data),
+			_pose(pose),
+			_covariance(generateCovarianceMatrix(rotVariance, transVariance)),
+			_info(info)
+	{
+	}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
-	bool isValid() const {return !_data.pose().isNull();}
+	SensorData & data() {return _data;}
 	const SensorData & data() const {return _data;}
-	int quality() const {return _quality;}
-	float time() const {return _time;} // seconds
-	int features() const {return _features;}
-	int localMapSize() const {return _localMapSize;}
+	const Transform & pose() const {return _pose;}
+	const cv::Mat & covariance() const {return _covariance;}
+	const OdometryInfo & info() const {return _info;}
+	double rotVariance() const {return uMax3(_covariance.at<double>(3,3), _covariance.at<double>(4,4), _covariance.at<double>(5,5));}
+	double transVariance() const {return uMax3(_covariance.at<double>(0,0), _covariance.at<double>(1,1), _covariance.at<double>(2,2));}
 
 private:
 	SensorData _data;
-	int _quality;
-	float _time; // seconds
-	int _features;
-	int _localMapSize;
+	Transform _pose;
+	cv::Mat _covariance;
+	OdometryInfo _info;
 };
 
 class OdometryResetEvent : public UEvent

corelib/include/rtabmap/core/OdometryInfo.h

@@ -0,0 +1,82 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYINFO_H_
+#define ODOMETRYINFO_H_
+
+#include <map>
+
+namespace rtabmap {
+
+class OdometryInfo
+{
+public:
+	OdometryInfo() :
+		lost(true),
+		matches(-1),
+		inliers(-1),
+		variance(-1),
+		features(-1),
+		localMapSize(-1),
+		timeEstimation(-1),
+		timeParticleFiltering(-1),
+		stamp(0),
+		interval(0),
+		distanceTravelled(0),
+		type(-1)
+	{}
+	bool lost;
+	int matches;
+	int inliers;
+	float variance;
+	int features;
+	int localMapSize;
+	float timeEstimation;
+	float timeParticleFiltering;
+	double stamp;
+	double interval;
+	Transform transform;
+	Transform transformFiltered;
+	float distanceTravelled;
+
+	int type; // 0=BOW, 1=Optical Flow, 2=ICP
+
+	// BOW odometry
+	std::multimap<int, cv::KeyPoint> words;
+	std::vector<int> wordMatches;
+	std::vector<int> wordInliers;
+	std::map<int, cv::Point3f> localMap;
+
+	// Optical Flow odometry
+	std::vector<cv::Point2f> refCorners;
+	std::vector<cv::Point2f> newCorners;
+	std::vector<int> cornerInliers;
+};
+
+}
+
+#endif /* ODOMETRYINFO_H_ */

corelib/include/rtabmap/core/OdometryThread.h

@@ -0,0 +1,72 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYTHREAD_H_
+#define ODOMETRYTHREAD_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <list>
+
+namespace rtabmap {
+
+class Odometry;
+
+class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
+public:
+	// take ownership of Odometry
+	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
+	virtual ~OdometryThread();
+
+protected:
+	virtual void handleEvent(UEvent * event);
+
+private:
+	void mainLoopKill();
+
+	//============================================================
+	// MAIN LOOP
+	//============================================================
+	void mainLoop();
+	void addData(const SensorData & data);
+	bool getData(SensorData & data);
+
+private:
+	USemaphore _dataAdded;
+	UMutex _dataMutex;
+	std::list<SensorData> _dataBuffer;
+	Odometry * _odometry;
+	unsigned int _dataBufferMaxSize;
+	bool _resetOdometry;
+};
+
+} // namespace rtabmap
+
+
+#endif /* ODOMETRYTHREAD_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -56,7 +56,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
  * 				DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
  * 			};
  * 			DummyVideoImageWidth dummyVideoImageWidth;
- * @endcode
+ * @endcode
  */
 #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
 	public: \
@@ -156,7 +156,7 @@ typedef std::pair<std::string, std::string> ParametersPair;
  * 		std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
  * @endcode
  * @see getDefaultParameters()
- * TODO Add a detailed example with simple classes
+ * TODO Add a detailed example with simple classes
  */
 class RTABMAP_EXP Parameters
 {
@@ -169,37 +169,46 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
-	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              int, 1, 	 "Data buffer size (0 min inf).");
+	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
+	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,                 unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
 	RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,   	 bool, true, "Add column header description to log files.");
-	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.")
+	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
-	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.9, 	"The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0, 	"The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
 
 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
-	RTABMAP_PARAM(Mem, ImageKept, 		        bool, true, 	"Keep images in db.");
-	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true, 	"Keep rehearsed ndoes in db.");
+	RTABMAP_PARAM(Mem, ImageKept, 		        bool, false, 	"Keep raw images in RAM.");
+	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
-	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, othwersize it is Localization mode.");
+	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, otherwise it is Localization mode.");
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
+	RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false,  "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
-	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location Ids, False=use input image ids.");
+	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
+	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
-	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, LaserScanVoxelSize,      float, 0.0,      "If > 0.0, voxelize laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM_COND(Kp, NNStrategy,       int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
+	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");
-	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"");
+	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
-	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8, 		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
+	RTABMAP_PARAM_COND(Kp, NndrRatio, 	     float, RTABMAP_NONFREE, 0.8, 0.9,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 	RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, true, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
@@ -240,14 +249,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
 
-	RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
 	RTABMAP_PARAM(GFTT, K, double, 0.04, "");
 
-	RTABMAP_PARAM(ORB, NFeatures,            int, 400,     "The maximum number of features to retain.");
 	RTABMAP_PARAM(ORB, ScaleFactor,          float,  1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
 	RTABMAP_PARAM(ORB, NLevels,              int, 1,       "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
 	RTABMAP_PARAM(ORB, EdgeThreshold,        int, 31,      "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
@@ -277,41 +284,76 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
 
 	// RGB-D SLAM
-	RTABMAP_PARAM(RGBD, Enabled,           bool, true, 	"");
-	RTABMAP_PARAM(RGBD, PoseScanMatching,  bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
-	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
-	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
-	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
-	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
+	RTABMAP_PARAM(RGBD, PoseScanMatching,         bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
+	RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.0,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.0,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
+	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
+	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
+	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0.0,  "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
+	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
+	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
+	RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
+	RTABMAP_PARAM(RGBD,  ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
+
+	// Graph optimization
+	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
+	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,      "Optimization iterations.");
+	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
+	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
+	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(RGBD, OptimizeRobust,            bool, true,  "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(Odom, LinearUpdate,           float, 0.0, 	"Min linear displacement to update odometry.");
-	RTABMAP_PARAM(Odom, AngularUpdate,          float, 0.0, 	"Min angular displacement to update odometry.");
-	RTABMAP_PARAM(Odom, MaxFeatures,            int, 0, 		"0 no limits.");
-	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(Odom, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
+	RTABMAP_PARAM(Odom, MaxFeatures,            int, 1000, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.1,     "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(Odom, Iterations,             int, 30, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
+	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
+	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,         "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Odom, MaxDepth,               float, 0,       "Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-	RTABMAP_PARAM(Odom, FeaturesRatio,          float, 0.0, 	"Minimum ratio of keypoints between the current image and the last image to compute odometry.");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(Odom, Holonomic, 		        bool, true,     "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
+	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
+	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
+	RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
+	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 5.0,     "PnP reprojection error.");
+	RTABMAP_PARAM(Odom, PnPFlags, 				int, 1,    	     "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Odom, ParticleFiltering, 		bool, false,     "Particle filtering to smooth the odometry trajectory.");
+	RTABMAP_PARAM(Odom, ParticleSize, 		    unsigned int, 400,  "Number of particles of the filter.");
+	RTABMAP_PARAM(Odom, ParticleNoiseT, 		float, 0.002,     "Noise (m) of translation components (x,y,z).");
+	RTABMAP_PARAM(Odom, ParticleLambdaT, 		float, 100,       "Lambda of translation components (x,y,z).");
+	RTABMAP_PARAM(Odom, ParticleNoiseR, 		float, 0.002,     "Noise (rad) of rotational components (roll,pitch,yaw).");
+	RTABMAP_PARAM(Odom, ParticleLambdaR, 		float, 100,       "Lambda of rotational components (roll,pitch,yaw).");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath,   "",         "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
+
+	// Odometry Mono
+	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, InitMinTranslation,     float, 0.1,  "Minimum translation required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02,  "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
+	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01,  "Maximum variance to add new points to local map.");
+
 
 	// Odometry common stuff between BOW and Optical Flow approaches
 	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
@@ -325,34 +367,40 @@ class RTABMAP_EXP Parameters
 
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
-	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxTranslation,  float, 0.2,      "Maximum ICP translation correction accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
-	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
-	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(LccBow, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+	RTABMAP_PARAM(LccBow, MinInliers,      int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.1,      "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
-	RTABMAP_PARAM(LccBow, MaxDepth,        float, 4.0, 		"Max depth of the words (0 means no limit).");
+	RTABMAP_PARAM(LccBow, RefineIterations,  int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
+	RTABMAP_PARAM(LccBow, PnPReprojError, 	 double, 5.0,    "PnP reprojection error.");
+	RTABMAP_PARAM(LccBow, PnPFlags, 	     int, 1,    	 "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
 	RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
 	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
-	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.7, 	"NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 600, 		"0 no limits.");
+	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 1000, 		"0 no limits.");
+	RTABMAP_PARAM(LccReextract, MaxDepth, 	float, 0.0,     "Max depth of the words (0 means no limit).");
 
-	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");
-	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 4.0, 	"Max cloud depth.");
-	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.01, 	"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp3, Decimation,      int, 4, 		"Depth image decimation.");
+	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 3.0, 	"Max cloud depth.");
+	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.025, 	"Voxel size to be used for ICP computation.");
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
-	RTABMAP_PARAM(LccIcp3, MaxFitness,      float, 1.0, 	"ICP 3D: Maximum fitness to accept the computed transform.");
-	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"ICP 3D: Use point to plane ICP.");
-	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"ICP 3D: Number of neighbors to compute normals for point to plane.");
+	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"Max iterations.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.2, 	"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
+	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
 
-	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
-	RTABMAP_PARAM(LccIcp2, MaxFitness,      float, 1.0, 			"ICP 2D: Maximum fitness to accept the computed transform.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, 	"Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, 		"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.025, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity
 	RTABMAP_PARAM(Stereo, WinSize,               int, 16,        "See cv::calcOpticalFlowPyrLK().");

corelib/include/rtabmap/core/ParticleFilter.h

@@ -0,0 +1,177 @@
+/*
+Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef PARTICLEFILTER_H_
+#define PARTICLEFILTER_H_
+
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
+
+
+namespace rtabmap {
+
+// taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/
+#define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */
+
+/*
+   RAND is a macro which returns a pseudo-random numbers from a uniform
+   distribution on the interval [0 1]
+*/
+#define RAND (rand())/((double) RAND_MAX)
+
+/*
+   RANDN is a macro which returns a pseudo-random numbers from a normal
+   distribution with mean zero and standard deviation one. This macro uses Box
+   Muller's algorithm
+*/
+#define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND))
+
+std::vector<double> cumSum(const std::vector<double> & v)
+{
+	std::vector<double> cum(v.size());
+	double sum = 0;
+	for(unsigned int i=0; i<v.size(); ++i)
+	{
+		cum[i] = v[i] + sum;
+		sum += v[i];
+	}
+	return cum;
+}
+
+std::vector<double> resample(const std::vector<double> & p, // particles
+							 const std::vector<double> & w, // weights
+							 bool normalizeWeights = false)
+{
+	std::vector<double> np; //new particles
+	if(p.size() != w.size() || p.size() == 0)
+	{
+		UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
+		return np;
+	}
+
+	std::vector<double> cs;
+	if(normalizeWeights)
+	{
+		double wSum = uSum(w);
+		std::vector<double> wNorm(w.size());
+		for(unsigned int i=0; i<w.size(); ++i)
+		{
+			wNorm[i] = w[i]/wSum;
+		}
+		cs = cumSum(wNorm); // cumulative sum
+	}
+	else
+	{
+		cs = cumSum(w); // cumulative sum
+	}
+	for(unsigned int j=0; j<cs.size(); ++j)
+	{
+		cs[j]/=cs.back();
+	}
+
+	np.resize(p.size());
+	for(unsigned int i=0; i<np.size(); ++i)
+	{
+		unsigned int index = 0;
+		double randnum = RAND;
+		for(unsigned int j=0; j<cs.size(); ++j)
+		{
+			if(randnum < cs[j])
+			{
+				index = j;
+				break;
+			}
+		}
+		np[i] = p[index];
+	}
+	return np;
+}
+
+
+class ParticleFilter
+{
+public:
+	ParticleFilter(unsigned int nParticles = 200,
+				   double noise = 0.1,
+				   double lambda = 10.0,
+				   double initValue = 0.0) :
+		noise_(noise),
+		lambda_(lambda)
+	{
+		particles_.resize(nParticles, initValue);
+	}
+
+	void init(double initValue = 0.0f)
+	{
+		particles_ = std::vector<double>(particles_.size(), initValue);
+	}
+
+	double filter(double val)
+	{
+		std::vector<double> weights(particles_.size(), 1);
+		double sumWeights = 0;
+		for(unsigned int i=0; i<particles_.size(); ++i)
+		{
+			// add noise to particle
+			particles_[i] += noise_ * RANDN;
+
+			// compute weight
+			double dist = fabs(particles_[i] - val);
+			//dist = sqrt(dist*dist);
+			double w = exp(-lambda_*dist);
+			if(uIsFinite(w) && w > 0)
+			{
+				weights[i] = w;
+			}
+			sumWeights += weights[i];
+		}
+
+
+		//normalize and compute estimated value
+		double value =0.0;
+		for(unsigned int i=0; i<weights.size(); ++i)
+		{
+			weights[i] /= sumWeights;
+			value += weights[i] * particles_[i];
+		}
+
+		//resample the particles
+		particles_ = resample(particles_, weights, false);
+
+		return value;
+	}
+
+private:
+	std::vector<double> particles_;
+	double noise_;
+	double lambda_;
+};
+
+}
+
+
+#endif /* PARTICLEFILTER_H_ */

corelib/include/rtabmap/core/Rtabmap.h

@@ -47,6 +47,9 @@ class EpipolarGeometry;
 class Memory;
 class BayesFilter;
 class Signature;
+namespace graph {
+class Optimizer;
+}
 
 class RTABMAP_EXP Rtabmap
 {
@@ -63,7 +66,10 @@ public:
 	virtual ~Rtabmap();
 
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
-	bool process(const SensorData & data); // for convenience
+	bool process(
+			const SensorData & data,
+			const Transform & odomPose,
+			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");
 	void init(const std::string & configFile = "", const std::string & databasePath = "");
@@ -71,10 +77,11 @@ public:
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
-	int getLoopClosureId() const;
-	int getRetrievedId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
 	int getLastLocationId() const;
-	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM() const; // working memory
 	std::set<int> getSTM() const; // short-term memory
 	int getWMSize() const; // working memory size
@@ -87,36 +94,57 @@ public:
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
+	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
+	float getLocalRadius() const {return _localRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
-	void triggerNewMap();
+	int triggerNewMap();
+	bool labelLocation(int id, const std::string & label);
+	bool setUserData(int id, const cv::Mat & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
-	void generateTOROGraph(const std::string & path, bool optimized, bool global);
+	void exportPoses(
+			const std::string & path,
+			bool optimized,
+			bool global,
+			int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
+	);
 	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
-	void deleteLocation(int locationId); // Only nodes in STM can be deleted
 	void rejectLoopClosure(int oldId, int newId);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
-			std::map<int, int> & mapIds,
 			bool optimized,
 			bool global) const;
 	void getGraph(std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
-			std::map<int, int> & mapIds,
 			bool optimized,
-			bool global);
+			bool global,
+			std::map<int, Signature> * signatures = 0);
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
+	void clearPath(int status); // -1=failed 0=idle/executing 1=success
+	bool computePath(int targetNode, bool global);
+	bool computePath(const Transform & targetPose); // only in current optimized map
+	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
+	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<int> getPathNextNodes() const;
+	int getPathCurrentGoalId() const;
+	unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
+	unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
+	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
+
+	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
+	std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -126,6 +154,13 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	std::map<int, Transform> optimizeGraph(
+			int fromId,
+			const std::set<int> & ids,
+			bool lookInDatabase,
+			std::multimap<int, Link> * constraints = 0) const;
+	void updateGoalIndex();
+	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -133,7 +168,7 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishLastSignature;
+	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
 	float _maxTimeAllowed; // in ms
@@ -141,6 +176,7 @@ private:
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
+	unsigned int _maxLocalRetrieved;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
 	bool _statisticLoggedHeaders;
@@ -149,33 +185,43 @@ private:
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
 	int _globalLoopClosureIcpType;
-	float _globalLoopClosureIcpMaxDistance;
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
-	float _localDetectRadius;
-	float _localDetectMaxNeighbors;
-	int _localDetectMaxDiffID;
-	int _toroIterations;
+	bool _scanMatchingIdsSavedInLinks;
+	float _localRadius;
+	float _localImmunizationRatio;
+	int _localDetectMaxGraphDepth;
+	float _localPathFilteringRadius;
+	bool _localPathOdomPosesUsed;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
+	float _optimizationMaxLinearError;
 	bool _reextractLoopClosureFeatures;
 	int _reextractNNType;
 	float _reextractNNDR;
 	int _reextractFeatureType;
 	int _reextractMaxWords;
+	float _reextractMaxDepth;
 	bool _startNewMapOnLoopClosure;
+	float _goalReachedRadius; // meters
+	bool _goalsSavedInUserData;
+	int _pathStuckIterations;
+	float _pathLinearVelocity;
+	float _pathAngularVelocity;
 
-	int _lcHypothesisId;
-	float _lcHypothesisValue;
-	int _retrievedId;
+	std::pair<int, float> _loopClosureHypothesis;
+	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
+	bool _someNodesHaveBeenTransferred;
+	float _distanceTravelled;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
-	ParametersMap _lastParameters;
+	graph::Optimizer * _graphOptimizer;
+	ParametersMap _modifiedParameters;
 
 	Memory * _memory;
 
@@ -191,7 +237,18 @@ private:
 	std::map<int, Transform> _optimizedPoses;
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
-	Transform _mapTransform; // for localization mode
+	Transform _lastLocalizationPose; // for localization mode
+	int _lastLocalizationNodeId; // for localization mode
+
+	// Planning stuff
+	int _pathStatus;
+	std::vector<std::pair<int,Transform> > _path;
+	std::set<unsigned int> _pathUnreachableNodes;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+	Transform _pathTransformToGoal;
+	int _pathStuckCount;
+
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UVariant.h>
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -58,48 +59,73 @@ class RtabmapEventCmd : public UEvent
 public:
 	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
 	enum Cmd {
-			kCmdInit,
+			kCmdInit,             // params: [string] database path + ParametersMap
 			kCmdResetMemory,
 			kCmdClose,
 			kCmdDumpMemory,
 			kCmdDumpPrediction,
-			kCmdGenerateDOTGraph, // params: path
-			kCmdGenerateDOTLocalGraph, // params: path, id, margin
-			kCmdGenerateTOROGraphLocal, // params: path, optimized
-			kCmdGenerateTOROGraphGlobal, // params: path, optimized
+			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
 			kCmdCleanDataBuffer,
-			kCmdPublish3DMapLocal, // params: optimized
-			kCmdPublish3DMapGlobal, // params: optimized
-			kCmdPublishTOROGraphGlobal, // params: optimized
-			kCmdPublishTOROGraphLocal, // params: optimized
+			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,
-			kCmdPause};
+			kCmdPause,
+			kCmdResume,
+			kCmdGoal,             // params: [string] label or [int] location ID
+			kCmdCancelGoal,
+			kCmdLabel             // params: [string] label, [int] location ID
+	};
 public:
-	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
+	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
-			_cmd(cmd),
-			_strValue(strValue),
-			_intValue(intValue),
-			_parameters(parameters){}
+			cmd_(cmd),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const UVariant & value4, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			value4_(value4),
+			parameters_(parameters){}
 
 	virtual ~RtabmapEventCmd() {}
-	Cmd getCmd() const {return _cmd;}
+	Cmd getCmd() const {return cmd_;}
 
-	void setStr(const std::string & str) {_strValue = str;}
-	const std::string & getStr() const {return _strValue;}
+	const UVariant & value1() const {return value1_;}
+	const UVariant & value2() const {return value2_;}
+	const UVariant & value3() const {return value3_;}
+	const UVariant & value4() const {return value4_;}
 
-	void setInt(int v) {_intValue = v;}
-	int getInt() const {return _intValue;}
-
-	const ParametersMap & getParameters() const {return _parameters;}
+	const ParametersMap & getParameters() const {return parameters_;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
 
 private:
-	Cmd _cmd;
-	std::string _strValue;
-	int _intValue;
-	ParametersMap _parameters;
+	Cmd cmd_;
+	UVariant value1_;
+	UVariant value2_;
+	UVariant value3_;
+	UVariant value4_;
+	ParametersMap parameters_;
 };
 
 class RtabmapEventInit : public UEvent
@@ -147,13 +173,11 @@ public:
 	RtabmapEvent3DMap(
 			const std::map<int, Signature> & signatures,
 			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
-			const std::map<int, int> & mapIds) :
+			const std::multimap<int, Link> & constraints) :
 		UEvent(0),
 		_signatures(signatures),
 		_poses(poses),
-		_constraints(constraints),
-		_mapIds(mapIds)
+		_constraints(constraints)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -161,7 +185,6 @@ public:
 	const std::map<int, Signature> & getSignatures() const {return _signatures;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
 	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -169,7 +192,56 @@ private:
 	std::map<int, Signature> _signatures;
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
-	std::map<int, int> _mapIds;
+};
+
+class RtabmapGlobalPathEvent : public UEvent
+{
+public:
+	RtabmapGlobalPathEvent():
+		UEvent(0) {}
+	RtabmapGlobalPathEvent(int goalId, const std::vector<std::pair<int, Transform> > & poses) :
+		UEvent(goalId),
+		_poses(poses) {}
+	RtabmapGlobalPathEvent(int goalId, const std::string & goalLabel, const std::vector<std::pair<int, Transform> > & poses) :
+			UEvent(goalId),
+			_goalLabel(goalLabel),
+			_poses(poses) {}
+
+	virtual ~RtabmapGlobalPathEvent() {}
+	int getGoal() const {return this->getCode();}
+	const std::string & getGoalLabel() const {return _goalLabel;}
+	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
+	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
+
+private:
+	std::string _goalLabel;
+	std::vector<std::pair<int, Transform> > _poses;
+};
+
+class RtabmapLabelErrorEvent : public UEvent
+{
+public:
+	RtabmapLabelErrorEvent(int id, const std::string & label):
+		UEvent(id),
+		_label(label){}
+
+	virtual ~RtabmapLabelErrorEvent() {}
+	int id() const {return this->getCode();}
+	const std::string & label() const {return _label;}
+	virtual std::string getClassName() const {return std::string("RtabmapLabelErrorEvent");}
+
+private:
+	std::string _label;
+};
+
+class RtabmapGoalStatusEvent : public UEvent
+{
+public:
+	RtabmapGoalStatusEvent(int status):
+		UEvent(status){}
+
+	virtual ~RtabmapGoalStatusEvent() {}
+	virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");}
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/OdometryEvent.h"
 
 #include <stack>
 
@@ -60,16 +61,15 @@ public:
 		kStateChangingParameters,
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
-		kStateGeneratingDOTGraph,
-		kStateGeneratingDOTLocalGraph,
-		kStateGeneratingTOROGraphLocal,
-		kStateGeneratingTOROGraphGlobal,
+		kStateExportingDOTGraph,
+		kStateExportingPoses,
 		kStateCleanDataBuffer,
-		kStatePublishingMapLocal,
-		kStatePublishingMapGlobal,
-		kStatePublishingTOROGraphLocal,
-		kStatePublishingTOROGraphGlobal,
-		kStateTriggeringMap
+		kStatePublishingMap,
+		kStateTriggeringMap,
+		kStateAddingUserData,
+		kStateSettingGoal,
+		kStateCancellingGoal,
+		kStateLabelling
 	};
 
 public:
@@ -79,7 +79,8 @@ public:
 
 	void clearBufferedData();
 	void setDetectorRate(float rate);
-	void setBufferSize(int bufferSize);
+	void setDataBufferSize(unsigned int bufferSize);
+	void createIntermediateNodes(bool enabled);
 
 protected:
 	virtual void handleEvent(UEvent * anEvent);
@@ -88,27 +89,32 @@ private:
 	virtual void mainLoop();
 	virtual void mainLoopKill();
 	void process();
-	void addData(const SensorData & data);
-	void getData(SensorData & data);
+	void addData(const OdometryEvent & odomEvent);
+	bool getData(OdometryEvent & data);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	void setDataBufferSize(int size);
-	void publishMap(bool optimized, bool full) const;
-	void publishTOROGraph(bool optimized, bool full) const;
+	void publishMap(bool optimized, bool full, bool graphOnly) const;
 
 private:
 	UMutex _stateMutex;
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;
 
-	std::list<SensorData> _dataBuffer;
+	std::list<OdometryEvent> _dataBuffer;
 	UMutex _dataMutex;
 	USemaphore _dataAdded;
-	int _dataBufferMaxSize;
+	unsigned int _dataBufferMaxSize;
 	float _rate;
+	bool _createIntermediateNodes;
 	UTimer * _frameRateTimer;
 
 	Rtabmap * _rtabmap;
 	bool _paused;
+	Transform lastPose_;
+	double _rotVariance;
+	double _transVariance;
+
+	cv::Mat _userData;
+	UMutex _userDataMutex;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/SensorData.h

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Transform.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
@@ -42,57 +44,143 @@ namespace rtabmap
 class RTABMAP_EXP SensorData
 {
 public:
-	SensorData(); // empty constructor
-	SensorData(const cv::Mat & image, int id = 0);
+	// empty constructor
+	SensorData();
 
-	// Metric constructor
-	SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id = 0);
+	// Appearance-only constructor
+	SensorData(
+			const cv::Mat & image,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
 
-	// Metric constructor + 2d depth
-	SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  const cv::Mat & depth2d,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id = 0);
+	// Mono constructor
+	SensorData(
+			const cv::Mat & image,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// RGB-D constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// RGB-D constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
+			int laserScanMaxPts,
+			float laserScanMaxRange,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras RGB-D constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras RGB-D constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
+			int laserScanMaxPts,
+			float laserScanMaxRange,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Stereo constructor
+	SensorData(
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Stereo constructor + 2d laser scan
+	SensorData(
+			const cv::Mat & laserScan,
+			int laserScanMaxPts,
+			float laserScanMaxRange,
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
 
 	virtual ~SensorData() {}
 
-	bool isValid() const {return !_image.empty();}
+	bool isValid() const {
+		return !(_id == 0 &&
+			_stamp == 0.0 &&
+			_laserScanMaxPts == 0 &&
+			_imageRaw.empty() &&
+			_imageCompressed.empty() &&
+			_depthOrRightRaw.empty() &&
+			_depthOrRightCompressed.empty() &&
+			_laserScanRaw.empty() &&
+			_laserScanCompressed.empty() &&
+			_cameraModels.size() == 0 &&
+			!_stereoCameraModel.isValid() &&
+			_userDataRaw.empty() &&
+			_userDataCompressed.empty() &&
+			_keypoints.size() == 0 &&
+			_descriptors.empty());
+	}
 
-	// use isValid() instead
-	RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead.");
-
-	const cv::Mat & image() const {return _image;}
 	int id() const {return _id;}
 	void setId(int id) {_id = id;}
+	double stamp() const {return _stamp;}
+	void setStamp(double stamp) {_stamp = stamp;}
+	int laserScanMaxPts() const {return _laserScanMaxPts;}
+	float laserScanMaxRange() const {return _laserScanMaxRange;}
 
-	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
-	void setPose(const Transform & pose) {_pose = pose;}
-	cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
-	cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
-	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
-	const cv::Mat & depth2d() const {return _depth2d;}
-	float fx() const {return _fx;}
-	float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
-	float cx() const {return _cx;}
-	float cy() const {return _cy;}
-	float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;}
-	float fyOrBaseline() const {return _fyOrBaseline;}
-	const Transform & pose() const {return _pose;}
-	const Transform & localTransform() const {return _localTransform;}
+	const cv::Mat & imageCompressed() const {return _imageCompressed;}
+	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
+	const cv::Mat & laserScanCompressed() const {return _laserScanCompressed;}
+
+	const cv::Mat & imageRaw() const {return _imageRaw;}
+	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
+	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
+	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
+	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
+	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
+	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
+	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
+
+	//for convenience
+	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
+	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
+
+	void uncompressData();
+	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0);
+	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
+
+	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
+	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
+
+	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
+	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 
 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
@@ -103,18 +191,25 @@ public:
 	const cv::Mat & descriptors() const {return _descriptors;}
 
 private:
-	cv::Mat _image;
 	int _id;
+	double _stamp;
+	int _laserScanMaxPts;
+	float _laserScanMaxRange;
 
-	// Metric stuff
-	cv::Mat _depthOrRightImage;
-	cv::Mat _depth2d;
-	float _fx;
-	float _fyOrBaseline;
-	float _cx;
-	float _cy;
-	Transform _pose;
-	Transform _localTransform;
+	cv::Mat _imageCompressed;          // compressed image
+	cv::Mat _depthOrRightCompressed;   // compressed image
+	cv::Mat _laserScanCompressed;      // compressed data
+
+	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
+	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
+	cv::Mat _laserScanRaw;      // CV_32FC2
+
+	std::vector<CameraModel> _cameraModels;
+	StereoCameraModel _stereoCameraModel;
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;

corelib/include/rtabmap/core/Signature.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Link.h>
 
 namespace rtabmap
 {
@@ -52,18 +53,12 @@ class RTABMAP_EXP Signature
 public:
 	Signature();
 	Signature(int id,
-			int mapId,
-			const std::multimap<int, cv::KeyPoint> & words,
-			const std::multimap<int, pcl::PointXYZ> & words3,
+			int mapId = -1,
+			int weight = 0,
+			double stamp = 0.0,
+			const std::string & label = std::string(),
 			const Transform & pose = Transform(),
-			const cv::Mat & depth2D = cv::Mat(),
-			const cv::Mat & image = cv::Mat(),
-			const cv::Mat & depth = cv::Mat(),
-			float fx = 0.0f,
-			float fy = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f,
-			const Transform & localTransform =Transform::getIdentity());
+			const SensorData & sensorData = SensorData());
 	virtual ~Signature();
 
 	/**
@@ -75,34 +70,33 @@ public:
 	int id() const {return _id;}
 	int mapId() const {return _mapId;}
 
-	void addNeighbors(const std::map<int, Transform> & neighbors);
-	void addNeighbor(int neighbor, const Transform & transform = Transform());
-	void removeNeighbor(int neighborId);
-	void removeNeighbors();
-	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
-	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
+	void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
+	int getWeight() const {return _weight;}
 
-	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
-	void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
-	void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
-	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
-	void changeLoopClosureId(int idFrom, int idTo);
+	void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
+	const std::string & getLabel() const {return _label;}
 
-	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
-	void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
-	void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
+	double getStamp() const {return _stamp;}
+
+	void addLinks(const std::list<Link> & links);
+	void addLinks(const std::map<int, Link> & links);
+	void addLink(const Link & link);
+
+	bool hasLink(int idTo) const;
+
+	void changeLinkIds(int idFrom, int idTo);
+
+	void removeLinks();
+	void removeLink(int idTo);
+	void removeVirtualLinks();
 
 	void setSaved(bool saved) {_saved = saved;}
-	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
-	void changeNeighborIds(int idFrom, int idTo);
+	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
 
-	const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
-	int getWeight() const {return _weight;}
-	const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
-	const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
+	const std::map<int, Link> & getLinks() const {return _links;}
 	bool isSaved() const {return _saved;}
-	bool isModified() const {return _modified || _neighborsModified;}
-	bool isNeighborsModified() const {return _neighborsModified;}
+	bool isModified() const {return _modified || _linksModified;}
+	bool isLinksModified() const {return _linksModified;}
 
 	//visual words stuff
 	void removeAllWords();
@@ -113,68 +107,40 @@ public:
 	void setEnabled(bool enabled) {_enabled = enabled;}
 	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	void setImageCompressed(const cv::Mat & bytes) {_imageCompressed = bytes;}
-	const cv::Mat & getImageCompressed() const {return _imageCompressed;}
-	void setImageRaw(const cv::Mat & image) {_imageRaw = image;}
-	const cv::Mat & getImageRaw() const {return _imageRaw;}
 
 	//metric stuff
 	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
-	void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
-	void setDepth2DCompressed(const cv::Mat & bytes) {_depth2DCompressed = bytes;}
-	void setLocalTransform(const Transform & t) {_localTransform = t;}
 	void setPose(const Transform & pose) {_pose = pose;}
-	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
-	const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
-	const cv::Mat & getDepth2DCompressed() const {return _depth2DCompressed;}
-	float getDepthFx() const {return _fx;}
-	float getDepthFy() const {return _fy;}
-	float getDepthCx() const {return _cx;}
-	float getDepthCy() const {return _cy;}
-	const Transform & getPose() const {return _pose;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
-	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setDepth2DRaw(const cv::Mat & depth2D) {_depth2DRaw = depth2D;}
-	const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
 
-	SensorData toSensorData();
-	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw);
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const;
+	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
+	const Transform & getPose() const {return _pose;}
+	cv::Mat getPoseCovariance() const;
+
+	SensorData & sensorData() {return _sensorData;}
+	const SensorData & sensorData() const {return _sensorData;}
 
 private:
 	int _id;
 	int _mapId;
-	std::map<int, Transform> _neighbors; // id, transform
+	double _stamp;
+	std::map<int, Link> _links; // id, transform
 	int _weight;
-	std::map<int, Transform> _loopClosureIds; // id, transform
-	std::map<int, Transform> _childLoopClosureIds; // id, transform
+	std::string _label;
 	bool _saved; // If it's saved to bd
 	bool _modified;
-	bool _neighborsModified; // Optimization when updating signatures in database
+	bool _linksModified; // Optimization when updating signatures in database
 
 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
 	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
-	cv::Mat _imageCompressed; // compressed image
 
-	cv::Mat _depthCompressed; // compressed image
-	cv::Mat _depth2DCompressed; // compressed data
-	float _fx;
-	float _fy;
-	float _cx;
-	float _cy;
 	Transform _pose;
-	Transform _localTransform; // camera_link -> base_link
-	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
 
-	cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
-	cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _depth2DRaw; // CV_32FC2
+	SensorData _sensorData;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Statistics.h

@@ -53,30 +53,42 @@ namespace rtabmap {
 class RTABMAP_EXP Statistics
 {
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
+	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
-	RTABMAP_STATS(Loop, ReactivateId,);
+	RTABMAP_STATS(Loop, Reactivate_id,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
-	RTABMAP_STATS(Loop, VisualInliers,);
-	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
-	RTABMAP_STATS(Loop, Last_loop_closure_child,);
+	RTABMAP_STATS(Loop, Visual_inliers,);
+	RTABMAP_STATS(Loop, Last_id,);
+	RTABMAP_STATS(Loop, Optimization_max_error, m);
 
-	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
-	RTABMAP_STATS(LocalLoop, Space_closure_id,);
-	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
-	RTABMAP_STATS(LocalLoop, Space_neighbors,);
-	RTABMAP_STATS(LocalLoop, Space_diff_id,);
+	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
+	RTABMAP_STATS(LocalLoop, Space_paths,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_visually,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
+
+	RTABMAP_STATS(OdomCorrection, Accepted,);
+	RTABMAP_STATS(OdomCorrection, Inliers,);
+	RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
+	RTABMAP_STATS(OdomCorrection, Variance,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
 	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Immunized_globally,);
+	RTABMAP_STATS(Memory, Immunized_locally,);
+	RTABMAP_STATS(Memory, Immunized_locally_max,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
+	RTABMAP_STATS(Memory, Rehearsal_id,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Distance_travelled, m);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);
@@ -101,9 +113,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
 	RTABMAP_STATS(TimingMem, Rehearsal, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
-	RTABMAP_STATS(TimingMem, Stereo_subpixel, ms);
+	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
-	RTABMAP_STATS(TimingMem, Keypoints_filtering, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
@@ -111,6 +122,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Indexed_words, words);
+	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
 	RTABMAP_STATS(Keypoint, Response_threshold,);
 
 public:
@@ -129,8 +142,7 @@ public:
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
 
-	void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
-	void setSignature(const Signature & s) {_signature = s;}
+	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
@@ -140,6 +152,8 @@ public:
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
+	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
+	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -147,8 +161,7 @@ public:
 	int loopClosureId() const {return _loopClosureId;}
 	int localLoopClosureId() const {return _localLoopClosureId;}
 
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
-	const Signature & getSignature() const {return _signature;}
+	const std::map<int, Signature> & getSignatures() const {return _signatures;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
@@ -158,6 +171,8 @@ public:
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
+	const std::vector<int> & localPath() const {return _localPath;}
+	int currentGoalId() const {return _currentGoalId;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -168,11 +183,7 @@ private:
 	int _loopClosureId;
 	int _localLoopClosureId;
 
-	// extended data start here...
-	std::map<int, int> _mapIds;
-
-	// Signature data
-	Signature _signature;
+	std::map<int, Signature> _signatures;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
@@ -184,6 +195,9 @@ private:
 	std::map<int, float> _likelihood;
 	std::map<int, float> _rawLikelihood;
 
+	std::vector<int> _localPath;
+	int _currentGoalId;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/Transform.h

@@ -31,6 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <vector>
 #include <string>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -44,25 +47,29 @@ public:
 	Transform(float r11, float r12, float r13, float o14,
 		      float r21, float r22, float r23, float o24,
 			  float r31, float r32, float r33, float o34);
+	// should have 3 rows, 4 cols and type CV_32FC1
+	Transform(const cv::Mat & transformationMatrix);
 	// x,y,z, roll,pitch,yaw
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y, theta
+	Transform(float x, float y, float theta);
 
-	float r11() const {return data_[0];}
-	float r12() const {return data_[1];}
-	float r13() const {return data_[2];}
-	float r21() const {return data_[4];}
-	float r22() const {return data_[5];}
-	float r23() const {return data_[6];}
-	float r31() const {return data_[8];}
-	float r32() const {return data_[9];}
-	float r33() const {return data_[10];}
+	float r11() const {return data()[0];}
+	float r12() const {return data()[1];}
+	float r13() const {return data()[2];}
+	float r21() const {return data()[4];}
+	float r22() const {return data()[5];}
+	float r23() const {return data()[6];}
+	float r31() const {return data()[8];}
+	float r32() const {return data()[9];}
+	float r33() const {return data()[10];}
 
-	float o14() const {return data_[3];}
-	float o24() const {return data_[7];}
-	float o34() const {return data_[11];}
+	float o14() const {return data()[3];}
+	float o24() const {return data()[7];}
+	float o34() const {return data()[11];}
 
-	float & operator[](int index) {return data_[index];}
-	const float & operator[](int index) const {return data_[index];}
+	float & operator[](int index) {return data()[index];}
+	const float & operator[](int index) const {return data()[index];}
 
 	bool isNull() const;
 	bool isIdentity() const;
@@ -70,25 +77,30 @@ public:
 	void setNull();
 	void setIdentity();
 
-	const float * data() const {return data_.data();}
-	float * data() {return data_.data();}
-	int size() const {return (int)data_.size();}
+	const float * data() const {return (const float *)data_.data;}
+	float * data() {return (float *)data_.data;}
+	int size() const {return 12;}
 
-	float & x() {return data_[3];}
-	float & y() {return data_[7];}
-	float & z() {return data_[11];}
-	const float & x() const {return data_[3];}
-	const float & y() const {return data_[7];}
-	const float & z() const {return data_[11];}
+	float & x() {return data()[3];}
+	float & y() {return data()[7];}
+	float & z() {return data()[11];}
+	const float & x() const {return data()[3];}
+	const float & y() const {return data()[7];}
+	const float & z() const {return data()[11];}
+
+	float theta() const;
 
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
 
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
 	float getNorm() const;
 	float getNormSquared() const;
+	float getDistance(const Transform & t) const;
+	float getDistanceSquared(const Transform & t) const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;
@@ -96,10 +108,25 @@ public:
 	bool operator==(const Transform & t) const;
 	bool operator!=(const Transform & t) const;
 
+	Eigen::Matrix4f toEigen4f() const;
+	Eigen::Matrix4d toEigen4d() const;
+	Eigen::Affine3f toEigen3f() const;
+	Eigen::Affine3d toEigen3d() const;
+
+	Eigen::Quaternionf getQuaternionf() const;
+	Eigen::Quaterniond getQuaterniond() const;
+
+public:
 	static Transform getIdentity();
+	static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
+	static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
+	static Transform fromEigen3f(const Eigen::Affine3f & matrix);
+	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
+	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
+	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
 private:
-	std::vector<float> data_;
+	cv::Mat data_;
 };
 
 RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);

corelib/include/rtabmap/core/UserDataEvent.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef USERDATAEVENT_H_
+#define USERDATAEVENT_H_
+
+#include <opencv2/opencv.hpp>
+#include <rtabmap/utilite/UEvent.h>
+
+namespace rtabmap
+{
+
+/**
+ * The user data event.
+ */
+class UserDataEvent : public UEvent
+{
+public:
+	UserDataEvent(const cv::Mat & data) :
+		UEvent(0),
+		data_(data)
+	{}
+	~UserDataEvent() {}
+	virtual std::string getClassName() const {return "UserDataEvent";}
+
+	const cv::Mat & data() const {return data_;}
+
+private:
+	cv::Mat data_;
+};
+
+}
+
+#endif /* USERDATAEVENT_H_ */
+

corelib/include/rtabmap/core/VWDictionary.h

@@ -41,6 +41,7 @@ namespace rtabmap
 
 class DBDriver;
 class VisualWord;
+class FlannIndex;
 
 class RTABMAP_EXP VWDictionary
 {
@@ -74,6 +75,8 @@ public:
 	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	unsigned int getIndexedWordsCount() const;
+	unsigned int getIndexMemoryUsed() const;
 	void setNNStrategy(NNStrategy strategy);
 	bool isIncremental() const {return _incrementalDictionary;}
 	void setIncrementalDictionary();
@@ -97,14 +100,16 @@ protected:
 
 private:
 	bool _incrementalDictionary;
+	bool _incrementalFlann;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
 	bool _newWordsComparedTogether;
 	int _lastWordId;
-	cv::flann::Index * _flannIndex;
+	FlannIndex * _flannIndex;
 	cv::Mat _dataTree;
 	NNStrategy _strategy;
 	std::map<int ,int> _mapIndexId;
+	std::map<int ,int> _mapIdIndex;
 	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
 	std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
 	std::set<int> _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary

corelib/include/rtabmap/core/VisualWord.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <opencv2/core/core.hpp>
+#include <map>
 
 namespace rtabmap
 {

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -1,436 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#ifndef UTIL3D_HPP_
-#define UTIL3D_HPP_
-
-#include <rtabmap/utilite/ULogger.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/filters/random_sample.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/filter.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/common/transforms.h>
-#include <pcl/common/common.h>
-#include <pcl/search/kdtree.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/segmentation/extract_clusters.h>
-
-namespace rtabmap{
-namespace util3d{
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr voxelize(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float voxelSize)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	UASSERT(voxelSize > 0.0f);
-	PointCloudPtr output(new PointCloud);
-	pcl::VoxelGrid<PointT> filter;
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr sampling(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	UASSERT(samples > 0);
-	PointCloudPtr output(new PointCloud);
-	pcl::RandomSample<PointT> filter;
-	filter.setSample(samples);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr passThrough(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	UASSERT(max > min);
-	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
-
-	PointCloudPtr output(new PointCloud);
-	pcl::PassThrough<PointT> filter;
-	filter.setFilterFieldName(axis);
-	filter.setFilterLimits(min, max);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
-	std::vector<int> indices;
-	pcl::removeNaNFromPointCloud<PointT>(*cloud, *output, indices);
-	return output;
-}
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
-	std::vector<int> indices;
-	pcl::removeNaNNormalsFromPointCloud<PointT>(*cloud, *output, indices);
-	return output;
-}
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const Transform & transform)
-{
-	typedef typename pcl::PointCloud<PointT> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
-	pcl::transformPointCloud<PointT>(*cloud, *output, transformToEigen4f(transform));
-	return output;
-}
-
-template<typename PointT>
-PointT transformPoint(
-		const PointT & pt,
-		const Transform & transform)
-{
-	return pcl::transformPoint(pt, transformToEigen3f(transform));
-}
-
-template<typename PointT>
-void segmentObstaclesFromGround(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		pcl::IndicesPtr & ground,
-		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
-		float groundNormalAngle,
-		int minClusterSize,
-		bool segmentFlatObstacles)
-{
-	ground.reset(new std::vector<int>);
-	obstacles.reset(new std::vector<int>);
-
-	// Find the ground
-	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
-			cloud,
-			groundNormalAngle,
-			Eigen::Vector4f(0,0,1,0),
-			normalRadiusSearch*2.0f,
-			Eigen::Vector4f(0,0,100,0));
-
-	if(segmentFlatObstacles)
-	{
-		int biggestFlatSurfaceIndex;
-		std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
-				cloud,
-				flatSurfaces,
-				normalRadiusSearch*2.0f,
-				minClusterSize,
-				std::numeric_limits<int>::max(),
-				&biggestFlatSurfaceIndex);
-
-
-		// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
-		ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-		Eigen::Vector4f min,max;
-		pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
-
-		for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
-		{
-			if((int)i!=biggestFlatSurfaceIndex)
-			{
-				Eigen::Vector4f centroid;
-				pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-				if(centroid[2] >= min[2] && centroid[2] <= max[2])
-				{
-					ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
-				}
-			}
-		}
-	}
-	else
-	{
-		ground = flatSurfaces;
-	}
-
-	if(ground->size() != cloud->size())
-	{
-		// Remove ground
-		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
-
-		//Cluster remaining stuff (obstacles)
-		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
-				cloud,
-				otherStuffIndices,
-				normalRadiusSearch*2.0f,
-				minClusterSize);
-
-		// merge indices
-		obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
-	}
-}
-
-template<typename PointT>
-void projectCloudOnXYPlane(
-		typename pcl::PointCloud<PointT>::Ptr & cloud)
-{
-	for(unsigned int i=0; i<cloud->size(); ++i)
-	{
-		cloud->at(i).z = 0;
-	}
-}
-
-template<typename PointT>
-pcl::IndicesPtr radiusFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float radiusSearch,
-		int minNeighborsInRadius)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return radiusFiltering<PointT>(cloud, indices, radiusSearch, minNeighborsInRadius);
-}
-
-template<typename PointT>
-pcl::IndicesPtr radiusFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float radiusSearch,
-		int minNeighborsInRadius)
-{
-	typedef typename pcl::search::KdTree<PointT> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-	KdTreePtr tree (new KdTree(false));
-
-	if(indices->size())
-	{
-		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
-		int oi = 0; // output iterator
-		tree->setInputCloud(cloud, indices);
-		for(unsigned int i=0; i<indices->size(); ++i)
-		{
-			std::vector<int> kIndices;
-			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
-			if(k > minNeighborsInRadius)
-			{
-				output->at(oi++) = indices->at(i);
-			}
-		}
-		output->resize(oi);
-		return output;
-	}
-	else
-	{
-		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
-		int oi = 0; // output iterator
-		tree->setInputCloud(cloud);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			std::vector<int> kIndices;
-			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
-			if(k > minNeighborsInRadius)
-			{
-				output->at(oi++) = i;
-			}
-		}
-		output->resize(oi);
-		return output;
-	}
-}
-
-template<typename PointT>
-pcl::IndicesPtr normalFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering<PointT>(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
-}
-
-template<typename PointT>
-pcl::IndicesPtr normalFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint)
-{
-	typedef typename pcl::search::KdTree<PointT> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-
-	pcl::NormalEstimation<PointT, pcl::Normal> ne;
-	ne.setInputCloud (cloud);
-	if(indices->size())
-	{
-		ne.setIndices(indices);
-	}
-
-	KdTreePtr tree (new KdTree(false));
-
-	if(indices->size())
-	{
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud(cloud);
-	}
-	ne.setSearchMethod (tree);
-
-	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-
-	ne.setRadiusSearch (radiusSearch);
-	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
-	{
-		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
-	}
-
-	ne.compute (*cloud_normals);
-
-	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
-	int oi = 0; // output iterator
-	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
-	for(unsigned int i=0; i<cloud_normals->size(); ++i)
-	{
-		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
-		float angle = pcl::getAngle3D(normal, v);
-		if(angle < angleMax)
-		{
-			output->at(oi++) = indices->size()!=0?indices->at(i):i;
-		}
-	}
-	output->resize(oi);
-
-	return output;
-}
-
-template<typename PointT>
-std::vector<pcl::IndicesPtr> extractClusters(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize,
-		int * biggestClusterIndex)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return extractClusters<PointT>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
-}
-
-template<typename PointT>
-std::vector<pcl::IndicesPtr> extractClusters(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize,
-		int * biggestClusterIndex)
-{
-	typedef typename pcl::search::KdTree<PointT> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-
-	KdTreePtr tree(new KdTree);
-	pcl::EuclideanClusterExtraction<PointT> ec;
-	ec.setClusterTolerance (clusterTolerance);
-	ec.setMinClusterSize (minClusterSize);
-	ec.setMaxClusterSize (maxClusterSize);
-	ec.setInputCloud (cloud);
-
-	if(indices->size())
-	{
-		ec.setIndices(indices);
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud(cloud);
-	}
-	ec.setSearchMethod (tree);
-
-	std::vector<pcl::PointIndices> cluster_indices;
-	ec.extract (cluster_indices);
-
-	int maxIndex=-1;
-	unsigned int maxSize = 0;
-	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
-	for(unsigned int i=0; i<cluster_indices.size(); ++i)
-	{
-		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
-
-		if(maxSize < cluster_indices[i].indices.size())
-		{
-			maxSize = cluster_indices[i].indices.size();
-			maxIndex = i;
-		}
-	}
-	if(biggestClusterIndex)
-	{
-		*biggestClusterIndex = maxIndex;
-	}
-
-	return output;
-}
-
-template<typename PointT>
-pcl::IndicesPtr extractNegativeIndices(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices)
-{
-	pcl::IndicesPtr output(new std::vector<int>);
-	pcl::ExtractIndices<PointT> extract;
-	extract.setInputCloud (cloud);
-	extract.setIndices(indices);
-	extract.setNegative(true);
-	extract.filter(*output);
-	return output;
-}
-
-} // util3d
-} // rtabmap
-#endif //UTIL3D_HPP_

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -0,0 +1,165 @@
+/*
+ * util3d_mapping.hpp
+ *
+ *  Created on: 2015-05-13
+ *      Author: mathieu
+ */
+
+#ifndef UTIL3D_MAPPING_HPP_
+#define UTIL3D_MAPPING_HPP_
+
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d.h>
+#include <pcl/common/common.h>
+#include <pcl/common/centroid.h>
+#include <pcl/common/io.h>
+
+namespace rtabmap{
+namespace util3d{
+
+template<typename PointT>
+void segmentObstaclesFromGround(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		pcl::IndicesPtr & ground,
+		pcl::IndicesPtr & obstacles,
+		float normalRadiusSearch,
+		float groundNormalAngle,
+		int minClusterSize,
+		bool segmentFlatObstacles)
+{
+	ground.reset(new std::vector<int>);
+	obstacles.reset(new std::vector<int>);
+
+	if(cloud->size())
+	{
+		// Find the ground
+		pcl::IndicesPtr flatSurfaces = normalFiltering(
+				cloud,
+				groundNormalAngle,
+				Eigen::Vector4f(0,0,1,0),
+				normalRadiusSearch*2.0f,
+				Eigen::Vector4f(0,0,100,0));
+
+		if(segmentFlatObstacles)
+		{
+			int biggestFlatSurfaceIndex;
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
+					cloud,
+					flatSurfaces,
+					normalRadiusSearch*2.0f,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+
+
+			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
+			ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+			Eigen::Vector4f min,max;
+			pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+			for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
+			{
+				if((int)i!=biggestFlatSurfaceIndex)
+				{
+					Eigen::Vector4f centroid;
+					pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
+					if(centroid[2] >= min[2] && centroid[2] <= max[2])
+					{
+						ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+					}
+				}
+			}
+		}
+		else
+		{
+			ground = flatSurfaces;
+		}
+
+		if(ground->size() != cloud->size())
+		{
+			// Remove ground
+			pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloud, ground);
+
+			//Cluster remaining stuff (obstacles)
+			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+					cloud,
+					otherStuffIndices,
+					normalRadiusSearch*2.0f,
+					minClusterSize);
+
+			// merge indices
+			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+		}
+	}
+}
+
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	if(cloud->size() == 0)
+	{
+		return;
+	}
+	pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+	segmentObstaclesFromGround<PointT>(cloud,
+			groundIndices,
+			obstaclesIndices,
+			cellSize,
+			groundNormalAngle,
+			minClusterSize);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane(groundCloud);
+		//voxelize to grid cell size
+		groundCloud = util3d::voxelize(groundCloud, cellSize);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane(obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
+	}
+
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloud->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
+		}
+	}
+}
+
+}
+}
+
+#endif /* UTIL3D_MAPPING_HPP_ */

corelib/include/rtabmap/core/util2d.h

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL2D_H_
+#define UTIL2D_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <opencv2/core/core.hpp>
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap
+{
+
+namespace util2d
+{
+
+cv::Mat RTABMAP_EXP disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
+
+cv::Mat RTABMAP_EXP disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02,
+		float maxCorrespondencesSlope = 0.1f);
+
+cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
+		float fx, float baseline,
+		int type = CV_32FC1); // CV_32FC1 or CV_16UC1
+
+cv::Mat RTABMAP_EXP depthFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		float fx,
+		float baseline,
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02);
+
+cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		const std::vector<unsigned char> & mask,
+		float maxSlope = 0.1f);
+
+cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		const std::vector<unsigned char> & mask,
+		float fx, float baseline);
+
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
+float RTABMAP_EXP getDepth(
+		const cv::Mat & depthImage,
+		float x, float y,
+		bool smoothing,
+		float maxZError = 0.02f);
+
+cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
+
+// Registration Depth to RGB
+cv::Mat RTABMAP_EXP registerDepth(
+		const cv::Mat & depth,
+		const cv::Mat & depthK,
+		const cv::Mat & colorK,
+		const rtabmap::Transform & transform);
+
+void RTABMAP_EXP fillRegisteredDepthHoles(
+		cv::Mat & depth,
+		bool vertical,
+		bool horizontal,
+		bool fillDoubleHoles = false);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL2D_H_ */

corelib/include/rtabmap/core/util3d.h

@@ -29,19 +29,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define UTIL3D_H_
 
 #include "rtabmap/core/RtabmapExp.h"
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <list>
-#include <string>
-#include <set>
 
-#include <rtabmap/core/Link.h>
-#include <rtabmap/utilite/UThread.h>
-#include <pcl/common/eigen.h>
-#include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/PolygonMesh.h>
+#include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/SensorData.h>
+#include <opencv2/core/core.hpp>
+#include <map>
+#include <list>
 
 namespace rtabmap
 {
@@ -49,47 +45,16 @@ namespace rtabmap
 namespace util3d
 {
 
-/**
- * Compress image or data
- *
- * Example compression:
- *   cv::Mat image;// an image
- *   CompressionThread ct(image);
- *   ct.start();
- *   ct.join();
- *   std::vector<unsigned char> bytes = ct.getCompressedData();
- *
- * Example uncompression
- *   std::vector<unsigned char> bytes;// a compressed image
- *   CompressionThread ct(bytes);
- *   ct.start();
- *   ct.join();
- *   cv::Mat image = ct.getUncompressedData();
- */
-class RTABMAP_EXP CompressionThread : public UThread
-{
-public:
-	// format : ".png" ".jpg" "" (empty is general)
-	CompressionThread(const cv::Mat & mat, const std::string & format = "");
-	CompressionThread(const cv::Mat & bytes, bool isImage);
-	const cv::Mat & getCompressedData() const {return compressedData_;}
-	cv::Mat & getUncompressedData() {return uncompressedData_;}
-protected:
-	virtual void mainLoop();
-private:
-	cv::Mat compressedData_;
-	cv::Mat uncompressedData_;
-	std::string format_;
-	bool image_;
-	bool compressMode_;
-};
+cv::Mat RTABMAP_EXP rgbFromCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
+		bool bgrOrder = true);
 
-cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
 cv::Mat RTABMAP_EXP depthFromCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
 		float & fx,
 		float & fy,
 		bool depth16U = true);
+
 void RTABMAP_EXP rgbdFromCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
 		cv::Mat & rgb,
@@ -99,52 +64,13 @@ void RTABMAP_EXP rgbdFromCloud(
 		bool bgrOrder = true,
 		bool depth16U = true);
 
-cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
-cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & transform);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const cv::Mat & disparity,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
-		const std::vector<cv::KeyPoint> & keypoints,
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform = Transform::getIdentity(),
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02);
-
-std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
-		const std::list<int> & wordIds,
-		const std::vector<cv::KeyPoint> & keypoints);
-
 pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,
 		float cx, float cy,
 		float fx, float fy,
 		bool smoothing,
-		float maxZError = 0.03f);
+		float maxZError = 0.02f);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
@@ -163,60 +89,50 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
 		const cv::Mat & imageDisparity,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
-cv::Mat RTABMAP_EXP disparityFromStereoImages(
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float voxelSize = 0.0f,
+		int samples = 0);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float voxelSize = 0.0f,
+		int samples = 0);
 
-cv::Mat RTABMAP_EXP disparityFromStereoImages(
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02,
-		float maxCorrespondencesSlope = 0.1f);
+pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth = 0,
+					const Transform & localTransform = Transform::getIdentity());
 
-cv::Mat RTABMAP_EXP depthFromStereoImages(
-		const cv::Mat & leftImage,
-		const cv::Mat & rightImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		float fx,
-		float baseline,
-		int flowWinSize = 9,
-		int flowMaxLevel = 4,
-		int flowIterations = 20,
-		double flowEps = 0.02);
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
 
-cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
-		const cv::Mat & leftImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask,
-		float maxSlope = 0.1f);
-
-cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
-		const cv::Mat & leftImage,
-		const std::vector<cv::Point2f> & leftCorners,
-		const std::vector<cv::Point2f> & rightCorners,
-		const std::vector<unsigned char> & mask,
-		float fx, float baseline);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform = Transform::getIdentity());
 
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
@@ -228,302 +144,10 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		float cx, float cy, float fx, float baseline);
 
-cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
-		float fx, float baseline,
-		int type = CV_32FC1);
-
-cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
-
-std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
-cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
-
-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
-
-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
-
-cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
-
-// remove depth by z axis
-void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
-									  const std::multimap<int, pcl::PointXYZ> & words2,
-									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
-									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
-
-void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
-									  const std::multimap<int, pcl::PointXYZ> & words2,
-									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
-									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
-
-void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
-									   const cv::Mat & depthImage1,
-									   const cv::Mat & depthImage2,
-									   float cx, float cy,
-									   float fx, float fy,
-									   float maxDepth,
-									   pcl::PointCloud<pcl::PointXYZ> & cloud1,
-									   pcl::PointCloud<pcl::PointXYZ> & cloud2);
-
-void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
-							   const pcl::PointCloud<pcl::PointXYZ> & cloud1,
-							   const pcl::PointCloud<pcl::PointXYZ> & cloud2,
-							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
-							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
-							   char depthAxis);
-void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
-							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
-							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
-							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
-							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
-							   char depthAxis);
-
-int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
-					 const std::multimap<int, pcl::PointXYZ> & wordsB);
-
-void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
-					pcl::PointCloud<pcl::PointXYZ> & inliers2,
-					float maxDepth,
-					char depthAxis,
-					bool removeDuplicates);
-
-Transform RTABMAP_EXP transformFromXYZCorrespondences(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
-		double inlierThreshold = 0.02,
-		int iterations = 100,
-		bool refineModel = false,
-		double refineModelSigma = 3.0,
-		int refineModelIterations = 10,
-		std::vector<int> * inliers = 0);
-
-Transform RTABMAP_EXP icp(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-		double maxCorrespondenceDistance,
-		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
-
-Transform RTABMAP_EXP icpPointToPlane(
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
-		double maxCorrespondenceDistance,
-		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
-
-Transform RTABMAP_EXP icp2D(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-		double maxCorrespondenceDistance,
-		int maximumIterations,
-		bool & hasConverged,
-		double & fitnessScore);
-
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20);
-
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20);
-
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		float smoothingSearchRadius = 0.025,
-		bool smoothingPolynomialFit = true);
-
-int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-							float maxDistance);
-
-void RTABMAP_EXP findCorrespondences(
-		const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
-
-void RTABMAP_EXP findCorrespondences(
-		const std::multimap<int, pcl::PointXYZ> & words1,
-		const std::multimap<int, pcl::PointXYZ> & words2,
-		pcl::PointCloud<pcl::PointXYZ> & inliers1,
-		pcl::PointCloud<pcl::PointXYZ> & inliers2,
-		float maxDepth,
-		std::set<int> * uniqueCorrespondences = 0);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform = Transform::getIdentity());
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform = Transform::getIdentity());
-
-inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
-{
-	Eigen::Matrix4f m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
-{
-	Eigen::Matrix4d m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-
-inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
-{
-	return Eigen::Affine3f(transformToEigen4f(transform));
-}
-
-inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
-{
-	return Eigen::Affine3d(transformToEigen4d(transform));
-}
-
-inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
-inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
-		const cv::Mat & image,
-		const cv::Mat & imageDepth,
-		float constant,
-		int fastThreshold=50,
-		bool fastNonmaxSuppression=true,
-		float maxDepth = 5.0f);
-
-pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
-		float gp3SearchRadius = 0.025,
-		float gp3Mu = 2.5,
-		int gp3MaximumNearestNeighbors = 100,
-		float gp3MaximumSurfaceAngle = M_PI/4,
-		float gp3MinimumAngle = M_PI/18,
-		float gp3MaximumAngle = 2*M_PI/3,
-		bool gp3NormalConsistency = false);
-
-std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
-		std::multimap<int, Link> & links,
-		int from,
-		int to);
-
-// <int, depth> depth=0 means infinite depth
-std::map<int, int> RTABMAP_EXP generateDepthGraph(
-		const std::multimap<int, Link> & links,
-		int fromId,
-		int depth = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, int> & depthGraph,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-bool RTABMAP_EXP saveTOROGraph(
-		const std::string & fileName,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints);
-
-bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
-		std::map<int, Transform> & poses,
-		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
-
-std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle,
-		bool keepLatest = true);
-
-std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle);
-
-bool RTABMAP_EXP occupancy2DFromCloud3D(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
-		float cellSize = 0.05f,
-		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
-
-cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
-		const std::map<int, Transform> & poses,
-		const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
-		float cellSize,
-		float & xMin,
-		float & yMin,
-		int fillEmptyRadius = 0,
-		float minMapSize = 0.0f);
-
-cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
-		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
-		float cellSize,
-		bool unknownSpaceFilled,
-		float & xMin,
-		float & yMin,
-		float minMapSize = 0.0f);
-
-void RTABMAP_EXP rayTrace(const cv::Point2i & start,
-		const cv::Point2i & end,
-		cv::Mat & grid,
-		bool stopOnObstacle);
-
-cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
+		const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
+		const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
 
 /**
  * @brief Concatenate a vector of indices to a single vector.
@@ -551,162 +175,12 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
 		const pcl::IndicesPtr & indicesA,
 		const pcl::IndicesPtr & indicesB);
 
-///////////////////
-// Templated PCL methods
-///////////////////
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr voxelize(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float voxelSize);
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr sampling(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		int samples);
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr passThrough(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max);
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud);
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud);
-
-template<typename PointT>
-typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const Transform & transform);
-
-template<typename PointT>
-PointT transformPoint(
-		const PointT & pt,
-		const Transform & transform);
-
-template<typename PointT>
-void segmentObstaclesFromGround(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		pcl::IndicesPtr & ground,
-		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
-		float groundNormalAngle,
-		int minClusterSize,
-		bool segmentFlatObstacles = false);
-
-template<typename PointT>
-void projectCloudOnXYPlane(
-		typename pcl::PointCloud<PointT>::Ptr & cloud);
-
-/**
- * For convenience.
- */
-template<typename PointT>
-pcl::IndicesPtr radiusFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float radiusSearch,
-		int minNeighborsInRadius);
-
-/**
- * @brief Wrapper of the pcl::RadiusOutlierRemoval class.
- *
- * Points in the cloud which have less than a minimum of neighbors in the
- * specified radius are filtered.
- * @param cloud the input cloud.
- * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
- * @param radiusSearch the radius in meter.
- * @param minNeighborsInRadius the minimum of neighbors to keep the point.
- * @return the indices of the points satisfying the parameters.
- */
-template<typename PointT>
-pcl::IndicesPtr radiusFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float radiusSearch,
-		int minNeighborsInRadius);
-
-/**
- * For convenience.
- */
-template<typename PointT>
-pcl::IndicesPtr normalFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint);
-
-/**
- * @brief Given a normal and a maximum angle error, keep all points of the cloud
- * respecting this normal.
- *
- * The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then
- * for each normal, the corresponding point is filtered if the
- * angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum
- * angle specified by the user.
- * @param cloud the input cloud.
- * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
- * @param angleMax the maximum angle.
- * @param normal the normal to which each point's normal is compared.
- * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
- * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
- * @return the indices of the points which respect the normal constraint.
- */
-template<typename PointT>
-pcl::IndicesPtr normalFiltering(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint);
-
-/**
- * For convenience.
- */
-template<typename PointT>
-std::vector<pcl::IndicesPtr> extractClusters(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize = std::numeric_limits<int>::max(),
-		int * biggestClusterIndex = 0);
-
-/**
- * @brief Wrapper of the pcl::EuclideanClusterExtraction class.
- *
- * Extract all clusters from a point cloud given a maximum cluster distance tolerance.
- * @param cloud the input cloud.
- * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
- * @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
- * @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
- * @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
- * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
- * @return the indices of each cluster found.
- */
-template<typename PointT>
-std::vector<pcl::IndicesPtr> extractClusters(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize = std::numeric_limits<int>::max(),
-		int * biggestClusterIndex = 0);
-
-template<typename PointT>
-pcl::IndicesPtr extractNegativeIndices(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
-		const pcl::IndicesPtr & indices);
+void RTABMAP_EXP savePCDWords(
+		const std::string & fileName,
+		const std::multimap<int, pcl::PointXYZ> & words,
+		const Transform & transform = Transform::getIdentity());
 
 } // namespace util3d
 } // namespace rtabmap
 
-#include "rtabmap/core/impl/util3d.hpp"
-
 #endif /* UTIL3D_H_ */

corelib/include/rtabmap/core/util3d_correspondences.h

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_CORRESPONDENCES_H_
+#define UTIL3D_CORRESPONDENCES_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/features2d/features2d.hpp>
+#include <set>
+#include <map>
+#include <list>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+
+void RTABMAP_EXP findCorrespondences(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
+
+void RTABMAP_EXP findCorrespondences(
+		const std::multimap<int, pcl::PointXYZ> & words1,
+		const std::multimap<int, pcl::PointXYZ> & words2,
+		pcl::PointCloud<pcl::PointXYZ> & inliers1,
+		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		float maxDepth,
+		std::vector<int> * uniqueCorrespondences = 0);
+
+void RTABMAP_EXP findCorrespondences(
+		const std::map<int, pcl::PointXYZ> & words1,
+		const std::map<int, pcl::PointXYZ> & words2,
+		pcl::PointCloud<pcl::PointXYZ> & inliers1,
+		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		float maxDepth,
+		std::vector<int> * correspondences = 0);
+
+// remove depth by z axis
+void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
+									  const std::multimap<int, pcl::PointXYZ> & words2,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
+									  const std::multimap<int, pcl::PointXYZ> & words2,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+									   const cv::Mat & depthImage1,
+									   const cv::Mat & depthImage2,
+									   float cx, float cy,
+									   float fx, float fy,
+									   float maxDepth,
+									   pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									   pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+							   const pcl::PointCloud<pcl::PointXYZ> & cloud1,
+							   const pcl::PointCloud<pcl::PointXYZ> & cloud2,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
+							   char depthAxis);
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
+							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
+							   char depthAxis);
+
+int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
+					 const std::multimap<int, pcl::PointXYZ> & wordsB);
+
+void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
+					pcl::PointCloud<pcl::PointXYZ> & inliers2,
+					float maxDepth,
+					char depthAxis,
+					bool removeDuplicates);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_CORRESPONDENCES_H_ */

corelib/include/rtabmap/core/util3d_features.h

@@ -0,0 +1,112 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_FEATURES_H_
+#define UTIL3D_FEATURES_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+#include <list>
+#include <map>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & disparity,
+		const StereoCameraModel & stereoCameraMode);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		float fx,
+		float baseline,
+		float cx,
+		float cy,
+		Transform localTransform = Transform::getIdentity(),
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02,
+		double maxCorrespondencesSlope = 0.0);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
+		const std::vector<cv::Point2f> & leftCorners,
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		float fx,
+		float baseline,
+		float cx,
+		float cy,
+		Transform localTransform = Transform::getIdentity(),
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02,
+		double maxCorrespondencesSlope = 0.0);
+
+std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
+		const std::multimap<int, cv::KeyPoint> & kpts,
+		const std::multimap<int, cv::KeyPoint> & previousKpts,
+		const CameraModel & cameraModel,
+		Transform & cameraTransform,
+		int pnpIterations = 100,
+		float pnpReprojError = 8.0f,
+		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
+		float ransacParam1 = 3.0f,
+		float ransacParam2 = 0.99f,
+		const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
+		double * variance = 0);
+
+std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
+		const std::list<int> & wordIds,
+		const std::vector<cv::KeyPoint> & keypoints);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_FEATURES_H_ */

corelib/include/rtabmap/core/util3d_filtering.h

@@ -0,0 +1,296 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_FILTERING_H_
+#define UTIL3D_FILTERING_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+#include <rtabmap/core/Transform.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float voxelSize);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float voxelSize);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize);
+
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		int samples);
+
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative = false);
+
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
+
+
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+
+/**
+ * For convenience.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * @brief Wrapper of the pcl::RadiusOutlierRemoval class.
+ *
+ * Points in the cloud which have less than a minimum of neighbors in the
+ * specified radius are filtered.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @param minNeighborsInRadius the minimum of neighbors to keep the point.
+ * @return the indices of the points satisfying the parameters.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+
+/**
+ * For convenience.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * @brief Given a normal and a maximum angle error, keep all points of the cloud
+ * respecting this normal.
+ *
+ * The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then
+ * for each normal, the corresponding point is filtered if the
+ * angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum
+ * angle specified by the user.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param angleMax the maximum angle.
+ * @param normal the normal to which each point's normal is compared.
+ * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
+ * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
+ * @return the indices of the points which respect the normal constraint.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * For convenience.
+ */
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+/**
+ * @brief Wrapper of the pcl::EuclideanClusterExtraction class.
+ *
+ * Extract all clusters from a point cloud given a maximum cluster distance tolerance.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
+ * @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
+ * @return the indices of each cluster found.
+ */
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices);
+pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_FILTERING_H_ */

corelib/include/rtabmap/core/util3d_mapping.h

@@ -0,0 +1,107 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_MAPPING_H_
+#define UTIL3D_MAPPING_H_
+
+#include "rtabmap/core/RtabmapExp.h"
+
+#include <opencv2/core/core.hpp>
+#include <map>
+#include <rtabmap/core/Transform.h>
+#include <pcl/pcl_base.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+void RTABMAP_EXP occupancy2DFromLaserScan(
+		const cv::Mat & scan,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		bool unknownSpaceFilled = false,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
+
+cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
+		const std::map<int, Transform> & poses,
+		const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
+		float cellSize,
+		float & xMin,
+		float & yMin,
+		float minMapSize = 0.0f,
+		bool erode = false);
+
+cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
+		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
+		float cellSize,
+		bool unknownSpaceFilled,
+		float & xMin,
+		float & yMin,
+		float minMapSize = 0.0f,
+		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
+
+void RTABMAP_EXP rayTrace(const cv::Point2i & start,
+		const cv::Point2i & end,
+		cv::Mat & grid,
+		bool stopOnObstacle);
+
+cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+
+void RTABMAP_EXP projectCloudOnXYPlane(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+
+// templated methods
+template<typename PointT>
+void segmentObstaclesFromGround(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		pcl::IndicesPtr & ground,
+		pcl::IndicesPtr & obstacles,
+		float normalRadiusSearch,
+		float groundNormalAngle,
+		int minClusterSize,
+		bool segmentFlatObstacles = false);
+
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize = 0.05f,
+		float groundNormalAngle = M_PI_4,
+		int minClusterSize = 20);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#include "rtabmap/core/impl/util3d_mapping.hpp"
+
+#endif /* UTIL3D_MAPPING_H_ */

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -0,0 +1,72 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_MOTION_ESTIMATION_H_
+#define UTIL3D_MOTION_ESTIMATION_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+Transform RTABMAP_EXP estimateMotion3DTo2D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
+			const CameraModel & cameraModel,
+			int minInliers = 10,
+			int iterations = 100,
+			double reprojError = 5.,
+			int flagsPnP = 0,
+			const Transform & guess = Transform::getIdentity(),
+			const std::map<int, pcl::PointXYZ> & words3B = std::map<int, pcl::PointXYZ>(),
+			double * varianceOut = 0,
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+
+Transform RTABMAP_EXP estimateMotion3DTo3D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, pcl::PointXYZ> & words3B,
+			int minInliers = 10,
+			double inliersDistance = 0.1,
+			int iterations = 100,
+			int refineIterations = 5,
+			double * varianceOut = 0,
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_TRANSFORMS_H_ */

corelib/include/rtabmap/core/util3d_registration.h

@@ -0,0 +1,111 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_REGISTRATION_H_
+#define UTIL3D_REGISTRATION_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+							float maxDistance);
+
+Transform RTABMAP_EXP transformFromXYZCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
+		double inlierThreshold = 0.02,
+		int iterations = 100,
+		bool refineModel = false,
+		double refineModelSigma = 3.0,
+		int refineModelIterations = 10,
+		std::vector<int> * inliers = 0,
+		double * variance = 0);
+
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);
+void RTABMAP_EXP computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut);
+
+Transform RTABMAP_EXP icp(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
+
+Transform RTABMAP_EXP icpPointToPlane(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
+
+Transform RTABMAP_EXP icp2D(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
+		const cv::Mat & depth,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
+		int decimation,
+		double maxDepth,
+		float voxel,
+		int samples,
+		const Transform & transform = Transform::getIdentity());
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_REGISTRATION_H_ */

corelib/include/rtabmap/core/util3d_surface.h

@@ -0,0 +1,123 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_SURFACE_H_
+#define UTIL3D_SURFACE_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/PolygonMesh.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/TextureMesh.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
+		float gp3SearchRadius = 0.025,
+		float gp3Mu = 2.5,
+		int gp3MaximumNearestNeighbors = 100,
+		float gp3MaximumSurfaceAngle = M_PI/4,
+		float gp3MinimumAngle = M_PI/18,
+		float gp3MaximumAngle = 2*M_PI/3,
+		bool gp3NormalConsistency = true);
+
+pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
+		const pcl::PolygonMesh::Ptr & mesh,
+		const std::map<int, Transform> & poses,
+		const std::map<int, CameraModel> & cameraModels,
+		const std::map<int, cv::Mat> & images,
+		const std::string & tmpDirectory = ".");
+
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int normalKSearch = 20);
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		int normalKSearch = 20);
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float searchRadius = 0.0f,
+		int polygonialOrder = 2,
+		int upsamplingMethod = 0, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
+		float upsamplingRadius = 0.0f,   // SAMPLE_LOCAL_PLANE
+		float upsamplingStep = 0.0f,     // SAMPLE_LOCAL_PLANE
+		int pointDensity = 0,            // RANDOM_UNIFORM_DENSITY
+		float dilationVoxelSize = 1.0f,  // VOXEL_GRID_DILATION
+		int dilationIterations = 0);     // VOXEL_GRID_DILATION
+
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+
+pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
+
+template<typename pointT>
+std::vector<pcl::Vertices> normalizePolygonsSide(
+		const pcl::PointCloud<pointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
+{
+	std::vector<pcl::Vertices> output(polygons.size());
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices polygon = polygons[i];
+		Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f n = (v1.cross(v2)).normalized();
+
+		Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
+
+		float result = n.dot(p);
+		if(result < 0)
+		{
+			//reverse vertices order
+			int tmp = polygon.vertices[0];
+			polygon.vertices[0] = polygon.vertices[2];
+			polygon.vertices[2] = tmp;
+		}
+
+		output[i] = polygon;
+	}
+	return output;
+}
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_SURFACE_H_ */

corelib/include/rtabmap/core/util3d_transforms.h

@@ -0,0 +1,66 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_TRANSFORMS_H_
+#define UTIL3D_TRANSFORMS_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & transform);
+
+pcl::PointXYZ RTABMAP_EXP transformPoint(
+		const pcl::PointXYZ & pt,
+		const Transform & transform);
+pcl::PointXYZRGB RTABMAP_EXP transformPoint(
+		const pcl::PointXYZRGB & pt,
+		const Transform & transform);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_TRANSFORMS_H_ */

corelib/src/BayesFilter.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "BayesFilter.h"
+#include "rtabmap/core/BayesFilter.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
@@ -75,7 +75,7 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 		bool valid = true;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			tmpValues[i] = std::atof((*iter).c_str());
+			tmpValues[i] = uStr2Float((*iter).c_str());
 			//UINFO("%d=%e", i, tmpValues[i]);
 			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
 			{
@@ -161,7 +161,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	// STEP 1 - Prediction : Prior*lastPosterior
 	_prediction = this->generatePrediction(memory, uKeys(likelihood));
 
-	ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
+	UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
 	//std::cout << "Prediction=" << _prediction << std::endl;
 
 	// Adjust the last posterior if some images were
@@ -260,7 +260,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				// Set high values (gaussians curves) to loop closure neighbors
 
 				// ADD prob for each neighbors
-				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
+				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true);
 				std::list<int> idsLoopMargin;
 				//filter neighbors in STM
 				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -273,7 +273,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 					{
 						if(iter->second == 0)
 						{
-							idsLoopMargin.push_back(iter->second);
+							idsLoopMargin.push_back(iter->first);
 						}
 						++iter;
 					}
@@ -289,9 +289,9 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
 				{
 					float sum = 0.0f; // sum values added
-					sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
+					sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap);
 					idsDone.insert(*iter);
-					this->normalize(prediction, i, sum, ids[0]<0);
+					this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0);
 				}
 			}
 			else
@@ -451,6 +451,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	UDEBUG("time getting removed ids = %fs", timer.restart());
 
 	int added = 0;
+	float epsilon = 0.00001f;
 	// get ids to update
 	std::set<int> idsToUpdate;
 	for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
@@ -460,47 +461,87 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			if(removedIds.find(oldIds[i]) != removedIds.end())
 			{
 				unsigned int cols = oldPrediction.cols;
+				int count = 0;
 				for(unsigned int j=0; j<cols; ++j)
 				{
-					if(((const float *)oldPrediction.data)[i + j*cols] != 0.0f &&
+					if(((const float *)oldPrediction.data)[i + j*cols] > epsilon &&
 					   j!=i &&
 					   removedIds.find(oldIds[j]) == removedIds.end())
 					{
 						//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
 						idsToUpdate.insert(oldIds[j]);
+						++count;
 					}
 				}
+				UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
 			}
 		}
 		if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
 		{
-			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0);
+			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true);
 			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
 			++added;
+			int count = 0;
 			for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
 			{
 				if(uContains(oldIdToIndexMap, iter->first) &&
 				   removedIds.find(iter->first) == removedIds.end())
 				{
 					idsToUpdate.insert(iter->first);
+					++count;
 				}
 			}
+			UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
 		}
 	}
-	UDEBUG("time getting ids to update = %fs", timer.restart());
+	UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
 
 	// update modified/added ids
 	int modified = 0;
+	std::set<int> idsDone;
 	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
 	{
-		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
-		int index = newIdToIndexMap.at(*iter);
-		float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
-		this->normalize(prediction, index, sum, newIds[0]<0);
-		++modified;
+		if(idsDone.find(*iter) == idsDone.end() && *iter > 0)
+		{
+			std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
+
+			std::list<int> idsLoopMargin;
+			//filter neighbors in STM
+			for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end();)
+			{
+				if(memory->isInSTM(jter->first))
+				{
+					neighbors.erase(jter++);
+				}
+				else
+				{
+					if(jter->second == 0)
+					{
+						idsLoopMargin.push_back(jter->first);
+					}
+					++jter;
+				}
+			}
+
+			// should at least have 1 id in idsMarginLoop
+			if(idsLoopMargin.size() == 0)
+			{
+				UFATAL("No 0 margin neighbor for signature %d !?!?", *iter);
+			}
+
+			// same neighbor tree for loop signatures (margin = 0)
+			for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
+			{
+				int index = newIdToIndexMap.at(*iter);
+				float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
+				idsDone.insert(*iter);
+				this->normalize(prediction, index, sum, newIds[0]<0);
+				++modified;
+			}
+		}
 	}
-	UDEBUG("time updating modified/added ids = %fs", timer.restart());
+	UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart());
 
 	//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
 	//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
@@ -510,15 +551,22 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	{
 		if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
 		{
-			for(int j=0; j<oldPrediction.cols; ++j)
+			for(int j=i; j<oldPrediction.cols; ++j)
 			{
-				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] != 0.0f)
+				if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] > epsilon)
 				{
 					//UDEBUG("i=%d, j=%d", i, j);
 					//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
 					//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
 					//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
-					((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+					float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
+					int ii = newIdToIndexMap.at(oldIds[i]);
+					int jj = newIdToIndexMap.at(oldIds[j]);
+					((float *)prediction.data)[ii + jj*prediction.cols] = v;
+					if(ii != jj)
+					{
+						((float *)prediction.data)[jj + ii*prediction.cols] = v;
+					}
 				}
 			}
 			++copied;
@@ -536,6 +584,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			for(int j=1; j<prediction.cols; j++)
 			{
 				((float*)prediction.data)[j*prediction.cols] = val;
+				((float*)prediction.data)[j] = _predictionLC[0];
 			}
 		}
 		else if(prediction.cols>0)

corelib/src/CMakeLists.txt

@@ -13,7 +13,10 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
+    CameraRGB.cpp
     CameraRGBD.cpp
+    CameraStereo.cpp
+    CameraModel.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp
@@ -24,14 +27,40 @@ SET(SRC_FILES
 	Features2d.cpp
 	Transform.cpp
 	
+	util2d.cpp
+	
 	util3d.cpp
-	Odometry.cpp
+	util3d_filtering.cpp
+	util3d_mapping.cpp
+	util3d_transforms.cpp
+	util3d_registration.cpp
+	util3d_surface.cpp
+	util3d_features.cpp
+	util3d_correspondences.cpp
+	util3d_motion_estimation.cpp
+	
 	SensorData.cpp
+	Graph.cpp
+	Compression.cpp
+	Link.cpp
+	
+	Odometry.cpp
+	OdometryThread.cpp
+	OdometryBOW.cpp
+	OdometryOpticalFlow.cpp
+	OdometryMono.cpp
+	OdometryICP.cpp
 	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp
 	toro3d/treeoptimizer3.cpp
 	
+	toro3d/posegraph2.cpp
+	toro3d/treeoptimizer2.cpp
+	
+	rtflann/ext/lz4.c
+	rtflann/ext/lz4hc.c
+	
 	sqlite3/sqlite3.c
 )
 
@@ -78,6 +107,90 @@ IF(OpenNI2_FOUND)
 	)
 ENDIF(OpenNI2_FOUND)
 
+IF(freenect2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FREENECT2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${freenect2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${freenect2_LIBRARIES}
+	)
+ENDIF(freenect2_FOUND)
+
+IF(DC1394_FOUND)
+	ADD_DEFINITIONS("-DWITH_DC1394")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${DC1394_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${DC1394_LIBRARIES}
+	)
+ENDIF(DC1394_FOUND)
+
+IF(FlyCapture2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${FlyCapture2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${FlyCapture2_LIBRARIES}
+	)
+ENDIF(FlyCapture2_FOUND)
+
+IF(G2O_FOUND)
+	ADD_DEFINITIONS("-DWITH_G2O")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${G2O_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${G2O_LIBRARIES}
+	)
+	#Newest versions require std11
+	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+	
+	SET(SRC_FILES
+    	${SRC_FILES}
+		vertigo/g2o/edge_se2MaxMixture.cpp
+		vertigo/g2o/edge_se2Switchable.cpp
+		vertigo/g2o/edge_se3Switchable.cpp
+		vertigo/g2o/edge_switchPrior.cpp
+		vertigo/g2o/types_g2o_robust.cpp
+		vertigo/g2o/vertex_switchLinear.cpp
+	)
+ENDIF(G2O_FOUND)
+
+IF(GTSAM_FOUND)
+	ADD_DEFINITIONS("-DWITH_GTSAM")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${GTSAM_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		gtsam
+	)
+ENDIF(GTSAM_FOUND)
+
+IF(cvsba_FOUND)
+	ADD_DEFINITIONS("-DWITH_CVSBA")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${cvsba_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${cvsba_LIBS}
+	)
+ENDIF(cvsba_FOUND)
+
 ####################################
 # Generate resources files
 ####################################
@@ -118,10 +231,17 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
 TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${LIBRARIES})
 
+SET_TARGET_PROPERTIES(
+     rtabmap_core
+   PROPERTIES
+     VERSION ${RTABMAP_VERSION}
+     SOVERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}
+)
+
 INSTALL(TARGETS rtabmap_core
-        RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
-        LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
-        ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel
+        ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT devel)
 		
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ 
 		DESTINATION "${INSTALL_INCLUDE_DIR}"

corelib/src/Camera.cpp

@@ -44,13 +44,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-Camera::Camera(float imageRate,
-		unsigned int imageWidth,
-		unsigned int imageHeight) :
+Camera::Camera(float imageRate, const Transform & localTransform) :
 	_imageRate(imageRate),
-	_imageWidth(imageWidth),
-	_imageHeight(imageHeight),
-	_frameRateTimer(new UTimer())
+	_localTransform(localTransform),
+	_targetImageSize(0,0),
+	_frameRateTimer(new UTimer()),
+	_seq(0)
 {
 }
 
@@ -62,292 +61,46 @@ Camera::~Camera()
 	}
 }
 
-void Camera::setImageSize(unsigned int width, unsigned int height)
+SensorData Camera::takeImage()
 {
-	_imageWidth = width;
-	_imageHeight = height;
-}
-
-void Camera::getImageSize(unsigned int & width, unsigned int & height)
-{
-	width = _imageWidth;
-	height = _imageHeight;
-}
-
-cv::Mat Camera::takeImage()
-{
-	cv::Mat img;
-	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
-	if(imageRate>0)
+	bool warnFrameRateTooHigh = false;
+	float actualFrameRate = 0;
+	if(_imageRate>0)
 	{
-		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
+		int sleepTime = (1000.0f/_imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
 		if(sleepTime > 2)
 		{
 			uSleep(sleepTime-2);
 		}
+		else if(sleepTime < 0)
+		{
+			warnFrameRateTooHigh = true;
+			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
+		}
 
 		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(_imageRate)-0.000001)
 		{
 			//
 		}
 
 		double slept = _frameRateTimer->getElapsedTime();
 		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_imageRate));
 	}
 
 	UTimer timer;
-	img = this->captureImage();
-	UDEBUG("Time capturing image = %fs", timer.ticks());
-	return img;
-}
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-CameraImages::CameraImages(const std::string & path,
-					 int startAt,
-					 bool refreshDir,
-					 float imageRate,
-					 unsigned int imageWidth,
-					 unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_path(path),
-	_startAt(startAt),
-	_refreshDir(refreshDir),
-	_count(0),
-	_dir(0)
-{
-
-}
-
-CameraImages::~CameraImages(void)
-{
-	if(_dir)
+	SensorData data  = this->captureImage();
+	if(warnFrameRateTooHigh)
 	{
-		delete _dir;
-	}
-}
-
-bool CameraImages::init()
-{
-	UDEBUG("");
-	if(_dir)
-	{
-		_dir->setPath(_path, "jpg ppm png bmp pnm");
+		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
+				_imageRate, actualFrameRate, timer.ticks());
 	}
 	else
 	{
-		_dir = new UDirectory(_path, "jpg ppm png bmp pnm");
+		UDEBUG("Time capturing image = %fs", timer.ticks());
 	}
-	_count = 0;
-	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
-	{
-		_path.append("/");
-	}
-	if(!_dir->isValid())
-	{
-		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
-	}
-	else if(_dir->getFileNames().size() == 0)
-	{
-		UWARN("Directory is empty \"%s\"", _path.c_str());
-	}
-	return _dir->isValid();
-}
-
-cv::Mat CameraImages::captureImage()
-{
-	cv::Mat img;
-	UDEBUG("");
-	if(_dir->isValid())
-	{
-		if(_refreshDir)
-		{
-			_dir->update();
-		}
-		if(_startAt == 0)
-		{
-			const std::list<std::string> & fileNames = _dir->getFileNames();
-			if(fileNames.size())
-			{
-				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
-				{
-					_lastFileName = *fileNames.rbegin();
-					std::string fullPath = _path + _lastFileName;
-					img = cv::imread(fullPath.c_str());
-				}
-			}
-		}
-		else
-		{
-			std::string fileName;
-			std::string fullPath;
-			fileName = _dir->getNextFileName();
-			if(fileName.size())
-			{
-				fullPath = _path + fileName;
-				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
-				{
-					fullPath = _path + fileName;
-				}
-				if(fileName.size())
-				{
-					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
-
-#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
-#else
-					img = cv::imread(fullPath.c_str(), -1);
-#endif
-					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
-							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
-
-					// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-					if(img.depth() != CV_8U)
-					{
-						// The depth should be 8U
-						UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
-						IplImage * i = cvLoadImage(fullPath.c_str());
-						img = cv::Mat(i, true);
-						cvReleaseImage(&i);
-					}
-				}
-			}
-		}
-	}
-	else
-	{
-		UWARN("Directory is not set, camera must be initialized.");
-	}
-
-	unsigned int w;
-	unsigned int h;
-	this->getImageSize(w, h);
-
-	if(!img.empty() &&
-	   w &&
-	   h &&
-	   w != (unsigned int)img.cols &&
-	   h != (unsigned int)img.rows)
-	{
-		cv::Mat resampled;
-		cv::resize(img, resampled, cv::Size(w, h));
-		img = resampled;
-	}
-	return img;
-}
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-CameraVideo::CameraVideo(int usbDevice,
-						 float imageRate,
-						 unsigned int imageWidth,
-						 unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_src(kUsbDevice),
-	_usbDevice(usbDevice)
-{
-
-}
-
-CameraVideo::CameraVideo(const std::string & filePath,
-						   float imageRate,
-						   unsigned int imageWidth,
-						   unsigned int imageHeight) :
-	Camera(imageRate, imageWidth, imageHeight),
-	_filePath(filePath),
-	_src(kVideoFile),
-	_usbDevice(0)
-{
-}
-
-CameraVideo::~CameraVideo()
-{
-	_capture.release();
-}
-
-bool CameraVideo::init()
-{
-	if(_capture.isOpened())
-	{
-		_capture.release();
-	}
-
-	if(_src == kUsbDevice)
-	{
-		unsigned int w;
-		unsigned int h;
-		this->getImageSize(w, h);
-
-		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, w, h);
-		_capture.open(_usbDevice);
-
-		if(w && h)
-		{
-			_capture.set(CV_CAP_PROP_FRAME_WIDTH, double(w));
-			_capture.set(CV_CAP_PROP_FRAME_HEIGHT, double(h));
-		}
-	}
-	else if(_src == kVideoFile)
-	{
-		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
-		_capture.open(_filePath.c_str());
-	}
-	else
-	{
-		ULOGGER_ERROR("Camera: Unknown source...");
-	}
-	if(!_capture.isOpened())
-	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
-		_capture.release();
-		return false;
-	}
-	return true;
-}
-
-cv::Mat CameraVideo::captureImage()
-{
-	cv::Mat img;
-	if(_capture.isOpened())
-	{
-		if(_capture.read(img))
-		{
-			unsigned int w;
-			unsigned int h;
-			this->getImageSize(w, h);
-
-			if(!img.empty() &&
-			   w &&
-			   h &&
-			   w != (unsigned int)img.cols &&
-			   h != (unsigned int)img.rows)
-			{
-				cv::Mat resampled;
-				cv::resize(img, resampled, cv::Size(w, h));
-				img = resampled;
-			}
-			else
-			{
-				// clone required
-				img = img.clone();
-			}
-		}
-		else if(_usbDevice)
-		{
-			UERROR("Camera has been disconnected!");
-		}
-	}
-	else
-	{
-		ULOGGER_WARN("The camera must be initialized before requesting an image.");
-	}
-	return img;
+	return data;
 }
 
 } // namespace rtabmap

corelib/src/CameraModel.cpp

@@ -0,0 +1,512 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace rtabmap {
+
+CameraModel::CameraModel() :
+		P_(cv::Mat::zeros(3, 4, CV_64FC1))
+{
+
+}
+
+CameraModel::CameraModel(
+		const std::string & cameraName,
+		const cv::Size & imageSize,
+		const cv::Mat & K,
+		const cv::Mat & D,
+		const cv::Mat & R,
+		const cv::Mat & P,
+		const Transform & localTransform) :
+		name_(cameraName),
+		imageSize_(imageSize),
+		K_(K),
+		D_(D),
+		R_(R),
+		P_(P),
+		localTransform_(localTransform)
+{
+	UASSERT(!name_.empty());
+	UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
+	UASSERT(K_.rows == 3 && K_.cols == 3);
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(R_.rows == 3 && R_.cols == 3);
+	UASSERT(P_.rows == 3 && P_.cols == 4);
+
+	// init rectification map
+	UINFO("Initialize rectify map");
+	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+}
+
+CameraModel::CameraModel(
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		const Transform & localTransform,
+		double Tx) :
+		K_(cv::Mat::eye(3, 3, CV_64FC1)),
+		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
+		R_(cv::Mat::eye(3, 3, CV_64FC1)),
+		P_(cv::Mat::eye(3, 4, CV_64FC1)),
+		localTransform_(localTransform)
+{
+	UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
+	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	P_.at<double>(0,0) = fx;
+	P_.at<double>(1,1) = fy;
+	P_.at<double>(0,2) = cx;
+	P_.at<double>(1,2) = cy;
+	P_.at<double>(0,3) = Tx;
+
+	K_.at<double>(0,0) = fx;
+	K_.at<double>(1,1) = fy;
+	K_.at<double>(0,2) = cx;
+	K_.at<double>(1,2) = cy;
+}
+
+CameraModel::CameraModel(
+		const std::string & name,
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		const Transform & localTransform,
+		double Tx) :
+		name_(name),
+		K_(cv::Mat::eye(3, 3, CV_64FC1)),
+		D_(cv::Mat::zeros(1, 5, CV_64FC1)),
+		R_(cv::Mat::eye(3, 3, CV_64FC1)),
+		P_(cv::Mat::eye(3, 4, CV_64FC1)),
+		localTransform_(localTransform)
+{
+	UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
+	UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
+	UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
+	UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
+	P_.at<double>(0,0) = fx;
+	P_.at<double>(1,1) = fy;
+	P_.at<double>(0,2) = cx;
+	P_.at<double>(1,2) = cy;
+	P_.at<double>(0,3) = Tx;
+
+	K_.at<double>(0,0) = fx;
+	K_.at<double>(1,1) = fy;
+	K_.at<double>(0,2) = cx;
+	K_.at<double>(1,2) = cy;
+}
+
+bool CameraModel::load(const std::string & directory, const std::string & cameraName)
+{
+	K_ = cv::Mat();
+	D_ = cv::Mat();
+	R_ = cv::Mat();
+	P_ = cv::Mat::zeros(3, 4, CV_64FC1);
+	mapX_ = cv::Mat();
+	mapY_ = cv::Mat();
+
+	std::string filePath = directory+"/"+cameraName+".yaml";
+	if(UFile::exists(filePath))
+	{
+		UINFO("Reading calibration file \"%s\"", filePath.c_str());
+		cv::FileStorage fs(filePath, cv::FileStorage::READ);
+
+		name_ = (int)fs["camera_name"];
+		imageSize_.width = (int)fs["image_width"];
+		imageSize_.height = (int)fs["image_height"];
+		UASSERT(!name_.empty());
+		//UASSERT(imageSize_.width > 0);
+		//UASSERT(imageSize_.height > 0);
+
+		// import from ROS calibration format
+		cv::FileNode n = fs["camera_matrix"];
+		int rows = (int)n["rows"];
+		int cols = (int)n["cols"];
+		std::vector<double> data;
+		n["data"] >> data;
+		UASSERT(rows*cols == (int)data.size());
+		UASSERT(rows == 3 && cols == 3);
+		K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+		n = fs["distortion_coefficients"];
+		rows = (int)n["rows"];
+		cols = (int)n["cols"];
+		data.clear();
+		n["data"] >> data;
+		UASSERT(rows*cols == (int)data.size());
+		UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
+		D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+		n = fs["rectification_matrix"];
+		rows = (int)n["rows"];
+		cols = (int)n["cols"];
+		data.clear();
+		n["data"] >> data;
+		UASSERT(rows*cols == (int)data.size());
+		UASSERT(rows == 3 && cols == 3);
+		R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+		n = fs["projection_matrix"];
+		rows = (int)n["rows"];
+		cols = (int)n["cols"];
+		data.clear();
+		n["data"] >> data;
+		UASSERT(rows*cols == (int)data.size());
+		UASSERT(rows == 3 && cols == 4);
+		P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+		fs.release();
+
+		if(imageSize_.height > 0 && imageSize_.width > 0)
+		{
+			// init rectification map
+			UINFO("Initialize rectify map");
+			cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+		}
+
+		return true;
+	}
+	else
+	{
+		UWARN("Could not load calibration file \"%s\".", filePath.c_str());
+	}
+	return false;
+}
+
+bool CameraModel::save(const std::string & directory) const
+{
+	std::string filePath = directory+"/"+name_+".yaml";
+	if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
+	{
+		UINFO("Saving calibration to file \"%s\"", filePath.c_str());
+		cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
+
+		// export in ROS calibration format
+
+		fs << "camera_name" << name_;
+		fs << "image_width" << imageSize_.width;
+		fs << "image_height" << imageSize_.height;
+
+		fs << "camera_matrix" << "{";
+		fs << "rows" << K_.rows;
+		fs << "cols" << K_.cols;
+		fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
+		fs << "}";
+
+		fs << "distortion_coefficients" << "{";
+		fs << "rows" << D_.rows;
+		fs << "cols" << D_.cols;
+		fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+		fs << "}";
+
+		// compaibility with ROS
+
+		if(D_.cols > 5)
+		{
+			fs << "distortion_model" << "rational_polynomial";
+		}
+		else
+		{
+			fs << "distortion_model" << "plumb_bob";
+		}
+
+		fs << "rectification_matrix" << "{";
+		fs << "rows" << R_.rows;
+		fs << "cols" << R_.cols;
+		fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+		fs << "}";
+
+		fs << "projection_matrix" << "{";
+		fs << "rows" << P_.rows;
+		fs << "cols" << P_.cols;
+		fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
+		fs << "}";
+
+		fs.release();
+
+		return true;
+	}
+	return false;
+}
+
+void CameraModel::scale(double scale)
+{
+	UASSERT(scale > 0.0);
+	// has only effect on K and P
+	imageSize_.width *= scale;
+	imageSize_.height *= scale;
+	K_.at<double>(0,0) *= scale;
+	K_.at<double>(1,1) *= scale;
+	K_.at<double>(0,2) *= scale;
+	K_.at<double>(1,2) *= scale;
+	P_.at<double>(0,0) *= scale;
+	P_.at<double>(1,1) *= scale;
+	P_.at<double>(0,2) *= scale;
+	P_.at<double>(1,2) *= scale;
+}
+
+double CameraModel::horizontalFOV() const
+{
+	if(imageWidth() > 0 && fx() > 0.0)
+	{
+		return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
+double CameraModel::verticalFOV() const
+{
+	if(imageHeight() > 0 && fy() > 0.0)
+	{
+		return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
+	}
+	return 0.0;
+}
+
+cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
+{
+	if(!mapX_.empty() && !mapY_.empty())
+	{
+		cv::Mat rectified;
+		cv::remap(raw, rectified, mapX_, mapY_, interpolation);
+		return rectified;
+	}
+	else
+	{
+		UERROR("Cannot rectify image because the rectify map is not initialized.");
+		return raw.clone();
+	}
+}
+
+//inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp
+cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
+{
+	UASSERT(raw.type() == CV_16UC1);
+	if(!mapX_.empty() && !mapY_.empty())
+	{
+		cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
+		for(int y=0; y<mapX_.rows; ++y)
+		{
+			for(int x=0; x<mapX_.cols; ++x)
+			{
+				cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
+				int xL = (int)floor(pt.x);
+				int xH = (int)ceil(pt.x);
+				int yL = (int)floor(pt.y);
+				int yH = (int)ceil(pt.y);
+				if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
+				{
+					const unsigned short & pLT = raw.at<unsigned short>(yL, xL);
+					const unsigned short & pRT = raw.at<unsigned short>(yL, xH);
+					const unsigned short & pLB = raw.at<unsigned short>(yH, xL);
+					const unsigned short & pRB = raw.at<unsigned short>(yH, xH);
+					if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
+					{
+						unsigned short avg = (pLT + pRT + pLB + pRB) / 4;
+						unsigned short thres = 0.01 * avg;
+						if( abs(pLT - avg) < thres &&
+							abs(pRT - avg) < thres &&
+							abs(pLB - avg) < thres &&
+							abs(pRB - avg) < thres)
+						{
+							//bilinear interpolation
+							float a = pt.x - (float)xL;
+							float c = pt.y - (float)yL;
+
+							//http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
+							rectified.at<unsigned short>(y,x) =
+									(raw.at<unsigned short>(yL, xL) * (1.f - a) + raw.at<unsigned short>(yL, xH) * a) * (1.f - c) +
+									(raw.at<unsigned short>(yH, xL) * (1.f - a) + raw.at<unsigned short>(yH, xH) * a) * c;
+						}
+					}
+				}
+			}
+		}
+		return rectified;
+	}
+	else
+	{
+		return raw.clone();
+	}
+}
+
+//
+//StereoCameraModel
+//
+void StereoCameraModel::setName(const std::string & name)
+{
+	name_=name;
+	left_.setName(name_+"_left");
+	right_.setName(name_+"_right");
+}
+
+bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
+{
+	name_ = cameraName;
+	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
+	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
+		//load rotation, translation
+		R_ = cv::Mat();
+		T_ = cv::Mat();
+
+		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		if(UFile::exists(filePath))
+		{
+			UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::READ);
+
+			name_ = (int)fs["camera_name"];
+
+			// import from ROS calibration format
+			cv::FileNode n = fs["rotation_matrix"];
+			int rows = (int)n["rows"];
+			int cols = (int)n["cols"];
+			std::vector<double> data;
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["translation_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 1);
+			T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["essential_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["fundamental_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			fs.release();
+
+			return true;
+		}
+		else
+		{
+			UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
+		}
+	}
+	return false;
+}
+bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
+{
+	if(left_.save(directory) && right_.save(directory))
+	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
+		std::string filePath = directory+"/"+name_+"_pose.yaml";
+		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
+		{
+			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
+
+			// export in ROS calibration format
+
+			fs << "camera_name" << name_;
+
+			fs << "rotation_matrix" << "{";
+			fs << "rows" << R_.rows;
+			fs << "cols" << R_.cols;
+			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+			fs << "}";
+
+			fs << "translation_matrix" << "{";
+			fs << "rows" << T_.rows;
+			fs << "cols" << T_.cols;
+			fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
+			fs << "}";
+
+			fs << "essential_matrix" << "{";
+			fs << "rows" << E_.rows;
+			fs << "cols" << E_.cols;
+			fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
+			fs << "}";
+
+			fs << "fundamental_matrix" << "{";
+			fs << "rows" << F_.rows;
+			fs << "cols" << F_.cols;
+			fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
+			fs << "}";
+
+			fs.release();
+
+			return true;
+		}
+	}
+	return false;
+}
+
+void StereoCameraModel::scale(double scale)
+{
+	left_.scale(scale);
+	right_.scale(scale);
+}
+
+Transform StereoCameraModel::stereoTransform() const
+{
+	if(!R_.empty() && !T_.empty())
+	{
+		return Transform(
+				R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
+				R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
+				R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
+	}
+	return Transform();
+}
+
+} /* namespace rtabmap */

corelib/src/CameraRGB.cpp

@@ -0,0 +1,402 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/CameraRGB.h"
+#include "rtabmap/core/DBDriver.h"
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <iostream>
+#include <cmath>
+
+namespace rtabmap
+{
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+CameraImages::CameraImages(const std::string & path,
+					 int startAt,
+					 bool refreshDir,
+					 bool rectifyImages,
+					 bool isDepth,
+					 float imageRate,
+					 const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_path(path),
+	_startAt(startAt),
+	_refreshDir(refreshDir),
+	_rectifyImages(rectifyImages),
+	_isDepth(isDepth),
+	_count(0),
+	_dir(0)
+{
+
+}
+
+CameraImages::~CameraImages(void)
+{
+	if(_dir)
+	{
+		delete _dir;
+	}
+}
+
+bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	_cameraName = cameraName;
+
+	UDEBUG("");
+	if(_dir)
+	{
+		_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
+	}
+	else
+	{
+		_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
+	}
+	_count = 0;
+	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
+	{
+		_path.append("/");
+	}
+	if(!_dir->isValid())
+	{
+		ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
+	}
+	else if(_dir->getFileNames().size() == 0)
+	{
+		UWARN("Directory is empty \"%s\"", _path.c_str());
+	}
+	else
+	{
+		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
+	}
+
+	// look for calibration files
+	if(!calibrationFolder.empty() && !cameraName.empty())
+	{
+		if(!_model.load(calibrationFolder, cameraName))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+					cameraName.c_str(), calibrationFolder.c_str());
+		}
+		else
+		{
+			UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+					_model.fx(),
+					_model.fy(),
+					_model.cx(),
+					_model.cy());
+		}
+	}
+
+	_model.setLocalTransform(this->getLocalTransform());
+	if(_rectifyImages && !_model.isValid())
+	{
+		UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
+		return false;
+	}
+
+	return _dir->isValid();
+}
+
+bool CameraImages::isCalibrated() const
+{
+	return _model.isValid();
+}
+
+std::string CameraImages::getSerial() const
+{
+	return _cameraName;
+}
+
+unsigned int CameraImages::imagesCount() const
+{
+	if(_dir)
+	{
+		return (unsigned int)_dir->getFileNames().size();
+	}
+	return 0;
+}
+
+std::vector<std::string> CameraImages::filenames() const
+{
+	if(_dir)
+	{
+		return uListToVector(_dir->getFileNames());
+	}
+	return std::vector<std::string>();
+}
+
+SensorData CameraImages::captureImage()
+{
+	cv::Mat img;
+	UDEBUG("");
+	if(_dir->isValid())
+	{
+		if(_refreshDir)
+		{
+			_dir->update();
+		}
+		if(_startAt == 0)
+		{
+			const std::list<std::string> & fileNames = _dir->getFileNames();
+			if(fileNames.size())
+			{
+				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
+				{
+					_lastFileName = *fileNames.rbegin();
+					std::string fullPath = _path + _lastFileName;
+					img = cv::imread(fullPath.c_str());
+				}
+			}
+		}
+		else
+		{
+			std::string fileName;
+			std::string fullPath;
+			fileName = _dir->getNextFileName();
+			if(fileName.size())
+			{
+				fullPath = _path + fileName;
+				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
+				{
+					fullPath = _path + fileName;
+				}
+				if(fileName.size())
+				{
+					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
+
+#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
+#else
+					img = cv::imread(fullPath.c_str(), -1);
+#endif
+					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
+							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
+
+					if(_isDepth)
+					{
+						if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
+						{
+							UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
+									"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
+									fileName.c_str());
+							img = cv::Mat();
+						}
+					}
+					else
+					{
+#if CV_MAJOR_VERSION < 3
+						// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
+						if(img.depth() != CV_8U)
+						{
+							// The depth should be 8U
+							UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
+							IplImage * i = cvLoadImage(fullPath.c_str());
+							img = cv::Mat(i, true);
+							cvReleaseImage(&i);
+						}
+#endif
+
+						if(img.channels()>3)
+						{
+							UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
+							cv::Mat out;
+							cv::cvtColor(img, out, CV_BGRA2BGR);
+							img = out;
+						}
+					}
+				}
+			}
+		}
+
+		if(!img.empty() && _model.isValid() && _rectifyImages)
+		{
+			img = _model.rectifyImage(img);
+		}
+	}
+	else
+	{
+		UWARN("Directory is not set, camera must be initialized.");
+	}
+
+	if(_isDepth)
+	{
+		return SensorData(cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
+	}
+	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
+}
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+CameraVideo::CameraVideo(
+		int usbDevice,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_rectifyImages(false),
+	_src(kUsbDevice),
+	_usbDevice(usbDevice)
+{
+
+}
+
+CameraVideo::CameraVideo(
+		const std::string & filePath,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_filePath(filePath),
+	_rectifyImages(rectifyImages),
+	_src(kVideoFile),
+	_usbDevice(0)
+{
+}
+
+CameraVideo::~CameraVideo()
+{
+	_capture.release();
+}
+
+bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	_guid.clear();
+	if(_capture.isOpened())
+	{
+		_capture.release();
+	}
+
+	if(_src == kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
+		_capture.open(_usbDevice);
+	}
+	else if(_src == kVideoFile)
+	{
+		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
+		_capture.open(_filePath.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("Camera: Unknown source...");
+	}
+	if(!_capture.isOpened())
+	{
+		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		_capture.release();
+		return false;
+	}
+	else
+	{
+		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+		if(guid != 0 && guid != 0xffffffff)
+		{
+			_guid = uFormat("%08x", guid);
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		{
+			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						_model.fx(),
+						_model.fy(),
+						_model.cx(),
+						_model.cy());
+			}
+		}
+		_model.setLocalTransform(this->getLocalTransform());
+		if(_rectifyImages && !_model.isValid())
+		{
+			UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
+			return false;
+		}
+	}
+	return true;
+}
+
+bool CameraVideo::isCalibrated() const
+{
+	return _model.isValid();
+}
+
+std::string CameraVideo::getSerial() const
+{
+	return _guid;
+}
+
+SensorData CameraVideo::captureImage()
+{
+	cv::Mat img;
+	if(_capture.isOpened())
+	{
+		if(_capture.read(img))
+		{
+			if(_model.isValid() && (_src != kVideoFile || _rectifyImages))
+			{
+				img = _model.rectifyImage(img);
+			}
+			else
+			{
+				// clone required
+				img = img.clone();
+			}
+		}
+		else if(_usbDevice)
+		{
+			UERROR("Camera has been disconnected!");
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+
+	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
+}
+
+} // namespace rtabmap

corelib/src/CameraThread.cpp

@@ -27,8 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -39,21 +41,13 @@ namespace rtabmap
 // ownership transferred
 CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
-		_cameraRGBD(0),
-		_seq(0)
+		_mirroring(false),
+		_colorOnly(false),
+		_stereoToDepth(false)
 {
 	UASSERT(_camera != 0);
 }
 
-// ownership transferred
-CameraThread::CameraThread(CameraRGBD * camera) :
-		_camera(0),
-		_cameraRGBD(camera),
-		_seq(0)
-{
-	UASSERT(_cameraRGBD != 0);
-}
-
 CameraThread::~CameraThread()
 {
 	join(true);
@@ -61,10 +55,6 @@ CameraThread::~CameraThread()
 	{
 		delete _camera;
 	}
-	if(_cameraRGBD)
-	{
-		delete _cameraRGBD;
-	}
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -73,78 +63,86 @@ void CameraThread::setImageRate(float imageRate)
 	{
 		_camera->setImageRate(imageRate);
 	}
-	if(_cameraRGBD)
-	{
-		_cameraRGBD->setImageRate(imageRate);
-	}
-}
-
-bool CameraThread::init()
-{
-	if(!this->isRunning())
-	{
-		_seq = 0;
-		if(_cameraRGBD)
-		{
-			return _cameraRGBD->init();
-		}
-		else
-		{
-			return _camera->init();
-		}
-
-		// Added sleep time to ignore first frames (which are darker)
-		uSleep(1000);
-	}
-	else
-	{
-		UERROR("Cannot initialize the camera because it is already running...");
-	}
-	return false;
 }
 
 void CameraThread::mainLoop()
 {
 	UTimer timer;
 	UDEBUG("");
-	cv::Mat rgb, depth;
-	float fx = 0.0f;
-	float fy = 0.0f;
-	float cx = 0.0f;
-	float cy = 0.0f;
-	if(_cameraRGBD)
-	{
-		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy);
-	}
-	else
-	{
-		rgb = _camera->takeImage();
-	}
+	SensorData data = _camera->takeImage();
 
-	if(!rgb.empty() && !this->isKilled())
+	if(!data.imageRaw().empty())
 	{
-		if(_cameraRGBD)
+		if(_colorOnly && !data.depthRaw().empty())
 		{
-			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
+			data.setDepthOrRightRaw(cv::Mat());
 		}
-		else
+		if(_mirroring && data.cameraModels().size() == 1)
 		{
-			this->post(new CameraEvent(rgb, ++_seq));
+			cv::Mat tmpRgb;
+			cv::flip(data.imageRaw(), tmpRgb, 1);
+			data.setImageRaw(tmpRgb);
+			if(data.cameraModels()[0].cx())
+			{
+				CameraModel tmpModel(
+						data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
+						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
+						data.cameraModels()[0].localTransform());
+				data.setCameraModel(tmpModel);
+			}
+			if(!data.depthRaw().empty())
+			{
+				cv::Mat tmpDepth;
+				cv::flip(data.depthRaw(), tmpDepth, 1);
+				data.setDepthOrRightRaw(tmpDepth);
+			}
 		}
+		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
+		{
+			cv::Mat depth = util2d::depthFromDisparity(
+					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().baseline());
+			data.setCameraModel(data.stereoCameraModel().left());
+			data.setDepthOrRightRaw(depth);
+			data.setStereoCameraModel(StereoCameraModel());
+		}
+
+		this->post(new CameraEvent(data, _camera->getSerial()));
 	}
 	else if(!this->isKilled())
 	{
-		if(_cameraRGBD)
-		{
-			UERROR("Retrieved data is empty! Stopping the camera...");
-		}
-		else
-		{
-			UWARN("no more images...");
-		}
+		UWARN("no more images...");
 		this->kill();
 		this->post(new CameraEvent());
 	}
 }
 
+void CameraThread::mainLoopKill()
+{
+	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
+	{
+		int i=20;
+		while(i-->0)
+		{
+			uSleep(100);
+			if(!this->isKilled())
+			{
+				break;
+			}
+		}
+		if(this->isKilled())
+		{
+			//still in killed state, maybe a deadlock
+			UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
+				   "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
+				   "Note that rtabmap should link on libusb of libfreenect2. "
+				   "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
+		}
+
+	}
+}
+
 } // namespace rtabmap

corelib/src/Compression.cpp

@@ -0,0 +1,264 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Compression.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/opencv.hpp>
+
+#include <zlib.h>
+
+namespace rtabmap {
+
+// format : ".png" ".jpg" "" (empty is general)
+CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
+	uncompressedData_(mat),
+	format_(format),
+	image_(!format.empty()),
+	compressMode_(true)
+{
+	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
+}
+// assume image
+CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
+	compressedData_(bytes),
+	image_(isImage),
+	compressMode_(false)
+{}
+void CompressionThread::mainLoop()
+{
+	if(compressMode_)
+	{
+		if(!uncompressedData_.empty())
+		{
+			if(image_)
+			{
+				compressedData_ = compressImage2(uncompressedData_, format_);
+			}
+			else
+			{
+				compressedData_ = compressData2(uncompressedData_);
+			}
+		}
+	}
+	else // uncompress
+	{
+		if(!compressedData_.empty())
+		{
+			if(image_)
+			{
+				uncompressedData_ = uncompressImage(compressedData_);
+			}
+			else
+			{
+				uncompressedData_ = uncompressData(compressedData_);
+			}
+		}
+	}
+	this->kill();
+}
+
+// ".png" or ".jpg"
+std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
+{
+	std::vector<unsigned char> bytes;
+	if(!image.empty())
+	{
+		if(image.type() == CV_32FC1)
+		{
+			//save in 8bits-4channel
+			cv::Mat bgra(image.size(), CV_8UC4, image.data);
+			cv::imencode(format, bgra, bytes);
+		}
+		else
+		{
+			cv::imencode(format, image, bytes);
+		}
+	}
+	return bytes;
+}
+
+// ".png" or ".jpg"
+cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
+{
+	std::vector<unsigned char> bytes = compressImage(image, format);
+	if(bytes.size())
+	{
+		return cv::Mat(1, (int)bytes.size(), CV_8UC1, bytes.data()).clone();
+	}
+	return cv::Mat();
+}
+
+cv::Mat uncompressImage(const cv::Mat & bytes)
+{
+	 cv::Mat image;
+	if(!bytes.empty())
+	{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+		image = cv::imdecode(bytes, -1);
+#endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
+	}
+	return image;
+}
+
+cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
+{
+	 cv::Mat image;
+	if(bytes.size())
+	{
+#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+#else
+		image = cv::imdecode(bytes, -1);
+#endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
+	}
+	return image;
+}
+
+std::vector<unsigned char> compressData(const cv::Mat & data)
+{
+	std::vector<unsigned char> bytes;
+	if(!data.empty())
+	{
+		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
+		uLong destLen = compressBound(sourceLen);
+		bytes.resize(destLen);
+		int errCode = compress(
+						(Bytef *)bytes.data(),
+						&destLen,
+						(const Bytef *)data.data,
+						sourceLen);
+
+		bytes.resize(destLen+3*sizeof(int));
+		*((int*)&bytes[destLen]) = data.rows;
+		*((int*)&bytes[destLen+sizeof(int)]) = data.cols;
+		*((int*)&bytes[destLen+2*sizeof(int)]) = data.type();
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+	}
+	return bytes;
+}
+
+cv::Mat compressData2(const cv::Mat & data)
+{
+	cv::Mat bytes;
+	if(!data.empty())
+	{
+		uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
+		uLong destLen = compressBound(sourceLen);
+		bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1);
+		int errCode = compress(
+						(Bytef *)bytes.data,
+						&destLen,
+						(const Bytef *)data.data,
+						sourceLen);
+		bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1));
+		*((int*)&bytes.data[destLen]) = data.rows;
+		*((int*)&bytes.data[destLen+sizeof(int)]) = data.cols;
+		*((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type();
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+	}
+	return bytes;
+}
+
+cv::Mat uncompressData(const cv::Mat & bytes)
+{
+	UASSERT(bytes.empty() || bytes.type() == CV_8UC1);
+	return uncompressData(bytes.data, bytes.cols*bytes.rows);
+}
+
+cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
+{
+	return uncompressData(bytes.data(), (unsigned long)bytes.size());
+}
+
+cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
+{
+	cv::Mat data;
+	if(bytes && size>=3*sizeof(int))
+	{
+		//last 3 int elements are matrix size and type
+		int height = *((int*)&bytes[size-3*sizeof(int)]);
+		int width = *((int*)&bytes[size-2*sizeof(int)]);
+		int type = *((int*)&bytes[size-1*sizeof(int)]);
+
+		// If the size is higher, it may be a wrong data format.
+		UASSERT_MSG(height>=0 && height<10000 &&
+				    width>=0 && width<10000,
+				    uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
+
+		data = cv::Mat(height, width, type);
+		uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
+
+		int errCode = uncompress(
+						(Bytef*)data.data,
+						&totalUncompressed,
+						(const Bytef*)bytes,
+						uLong(size));
+
+		if(errCode == Z_MEM_ERROR)
+		{
+			UERROR("Z_MEM_ERROR : Insufficient memory.");
+		}
+		else if(errCode == Z_BUF_ERROR)
+		{
+			UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
+		}
+		else if(errCode == Z_DATA_ERROR)
+		{
+			UERROR("Z_DATA_ERROR : The compressed data (referenced by source) was corrupted.");
+		}
+	}
+	return data;
+}
+
+} /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -28,17 +28,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/DBDriver.h"
 
 #include "rtabmap/core/Signature.h"
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
+#include "DBDriverSqlite3.h"
 
 namespace rtabmap {
 
+DBDriver * DBDriver::create(const ParametersMap & parameters)
+{
+	// well, we only have Sqlite3 database type for now :P
+	return new DBDriverSqlite3(parameters);
+}
+
 DBDriver::DBDriver(const ParametersMap & parameters) :
-	_emptyTrashesTime(0)
+	_emptyTrashesTime(0),
+	_timestampUpdate(true)
 {
 	this->parseParameters(parameters);
 }
@@ -98,6 +106,15 @@ long DBDriver::getMemoryUsed() const
 	return bytes;
 }
 
+std::string DBDriver::getDatabaseVersion() const
+{
+	std::string version = "0.0.0";
+	_dbSafeAccessMutex.lock();
+	getDatabaseVersionQuery(version);
+	_dbSafeAccessMutex.unlock();
+	return version;
+}
+
 void DBDriver::mainLoop()
 {
 	this->emptyTrashes();
@@ -242,7 +259,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{
@@ -272,7 +289,7 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{
@@ -281,6 +298,23 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 	}
 }
 
+void DBDriver::addLink(const Link & link)
+{
+	_dbSafeAccessMutex.lock();
+	this->addLinkQuery(link);
+	_dbSafeAccessMutex.unlock();
+}
+void DBDriver::removeLink(int from, int to)
+{
+	this->executeNoResult(uFormat("DELETE FROM Link WHERE from_id=%d and to_id=%d", from, to).c_str());
+}
+void DBDriver::updateLink(const Link & link)
+{
+	_dbSafeAccessMutex.lock();
+	this->updateLinkQuery(link);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::load(VWDictionary * dictionary) const
 {
 	_dbSafeAccessMutex.lock();
@@ -306,8 +340,6 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
 	bool valueFound = false;
 	_trashesMutex.lock();
 	{
-		_dbSafeAccessMutex.lock();
-		_dbSafeAccessMutex.unlock();
 		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
 		{
 			valueFound = false;
@@ -360,8 +392,6 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	{
 		if(_trashVisualWords.size())
 		{
-			_dbSafeAccessMutex.lock();
-			_dbSafeAccessMutex.unlock();
 			for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
 			{
 				wIter = _trashVisualWords.find(*iter);
@@ -393,100 +423,302 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-//TODO Check also in the trash ?
-void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const
+void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
 {
+	// Don't look in the trash, we assume that if we want to load
+	// data of a signature, it is not in thrash! Print an error if so.
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			UASSERT(*iter != 0);
+			UASSERT_MSG(!uContains(_trashSignatures, (*iter)->id()), uFormat("Signature %d should not be used when transferred to trash!!!!", (*iter)->id()).c_str());
+		}
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
-	this->loadNodeDataQuery(signatures, loadMetricData);
+	this->loadNodeDataQuery(signatures);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getNodeData(
 		int signatureId,
-		cv::Mat & imageCompressed,
-		cv::Mat & depthCompressed,
-		cv::Mat & depth2dCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform) const
+		SensorData & data) const
 {
-	_dbSafeAccessMutex.lock();
-	this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, depth2dCompressed, fx, fy, cx, cy, localTransform);
-	_dbSafeAccessMutex.unlock();
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
+		{
+			data = (SensorData)s->sensorData();
+			found = true;
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		std::list<Signature *> signatures;
+		Signature tmp(signatureId);
+		signatures.push_back(&tmp);
+		loadNodeDataQuery(signatures);
+		data = signatures.front()->sensorData();
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
-void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
+bool DBDriver::getNodeInfo(int signatureId,
+		Transform & pose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp) const
 {
-	_dbSafeAccessMutex.lock();
-	this->getNodeDataQuery(signatureId, imageCompressed);
-	_dbSafeAccessMutex.unlock();
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		pose = _trashSignatures.at(signatureId)->getPose();
+		mapId = _trashSignatures.at(signatureId)->mapId();
+		weight = _trashSignatures.at(signatureId)->getWeight();
+		label = _trashSignatures.at(signatureId)->getLabel();
+		stamp = _trashSignatures.at(signatureId)->getStamp();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
 }
 
-//TODO Check also in the trash ?
-void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
+void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
 {
-	_dbSafeAccessMutex.lock();
-	this->getPoseQuery(signatureId, pose, mapId);
-	_dbSafeAccessMutex.unlock();
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		UASSERT(s != 0);
+		for(std::map<int, Link>::const_iterator nIter = s->getLinks().begin();
+				nIter!=s->getLinks().end();
+				++nIter)
+		{
+			if(type == Link::kUndef || nIter->second.type() == type)
+			{
+				links.insert(*nIter);
+			}
+		}
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->loadLinksQuery(signatureId, links, type);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
-void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
-{
-	_dbSafeAccessMutex.lock();
-	this->loadNeighborsQuery(signatureId, neighbors);
-	_dbSafeAccessMutex.unlock();
-}
-
-//TODO Check also in the trash ?
 void DBDriver::getWeight(int signatureId, int & weight) const
 {
-	_dbSafeAccessMutex.lock();
-	this->getWeightQuery(signatureId, weight);
-	_dbSafeAccessMutex.unlock();
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		weight = _trashSignatures.at(signatureId)->getWeight();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->getWeightQuery(signatureId, weight);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
-void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
-{
-	_dbSafeAccessMutex.lock();
-	this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
-	_dbSafeAccessMutex.unlock();
-}
-
-//TODO Check also in the trash ?
 void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			bool hasNeighbors = !ignoreChildren;
+			if(ignoreChildren)
+			{
+				for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
+						nIter!=sIter->second->getLinks().end();
+						++nIter)
+				{
+					if(nIter->second.type() == Link::kNeighbor)
+					{
+						hasNeighbors = true;
+						break;
+					}
+				}
+			}
+			if(hasNeighbors)
+			{
+				ids.insert(sIter->first);
+			}
+		}
+
+		std::vector<int> keys = uKeys(_trashSignatures);
+
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getAllNodeIdsQuery(ids, ignoreChildren);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
+void DBDriver::getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks) const
+{
+	_dbSafeAccessMutex.lock();
+	this->getAllLinksQuery(links, ignoreNullLinks);
+	_dbSafeAccessMutex.unlock();
+
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator iter=_trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
+		{
+			links.erase(iter->first);
+			for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
+				jter!=iter->second->getLinks().end();
+				++jter)
+			{
+				if(!ignoreNullLinks || jter->second.isValid())
+				{
+					links.insert(std::make_pair(iter->first, jter->second));
+				}
+			}
+		}
+	}
+	_trashesMutex.unlock();
+}
+
 void DBDriver::getLastNodeId(int & id) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		id = _trashSignatures.rbegin()->first;
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getLastIdQuery("Node", id);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getLastWordId(int & id) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashVisualWords.size())
+	{
+		id = _trashVisualWords.rbegin()->first;
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getLastIdQuery("Word", id);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
 {
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		ni = _trashSignatures.at(signatureId)->getWords().size();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->getInvertedIndexNiQuery(signatureId, ni);
+		_dbSafeAccessMutex.unlock();
+	}
+}
+
+void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
+{
+	if(!label.empty())
+	{
+		int idFound = 0;
+		// look in the trash
+		_trashesMutex.lock();
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			if(sIter->second->getLabel().compare(label) == 0)
+			{
+				idFound = sIter->first;
+				break;
+			}
+		}
+		_trashesMutex.unlock();
+
+		// then look in the database
+		if(idFound == 0)
+		{
+			_dbSafeAccessMutex.lock();
+			this->getNodeIdByLabelQuery(label, id);
+			_dbSafeAccessMutex.unlock();
+		}
+		else
+		{
+			id = idFound;
+		}
+	}
+	else
+	{
+		UWARN("Can't search with an empty label!");
+	}
+}
+
+void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+	{
+		if(!sIter->second->getLabel().empty())
+		{
+			labels.insert(std::make_pair(sIter->first, sIter->second->getLabel()));
+		}
+	}
+	_trashesMutex.unlock();
+
+	// then look in the database
 	_dbSafeAccessMutex.lock();
-	this->getInvertedIndexNiQuery(signatureId, ni);
+	this->getAllLabelsQuery(labels);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -507,4 +739,155 @@ void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce
 	}
 }
 
+void DBDriver::generateGraph(
+		const std::string & fileName,
+		const std::set<int> & idsInput,
+		const std::map<int, Signature *> & otherSignatures)
+{
+	if(this->isConnected())
+	{
+		if(!fileName.empty())
+		{
+			FILE* fout = 0;
+			#ifdef _MSC_VER
+				fopen_s(&fout, fileName.c_str(), "w");
+			#else
+				fout = fopen(fileName.c_str(), "w");
+			#endif
+
+			 if (!fout)
+			 {
+				 UERROR("Cannot open file %s!", fileName.c_str());
+				 return;
+			 }
+
+			 std::set<int> ids;
+			 if(idsInput.size() == 0)
+			 {
+				 this->getAllNodeIds(ids);
+				 UDEBUG("ids.size()=%d", ids.size());
+				 for(std::map<int, Signature*>::const_iterator iter=otherSignatures.begin(); iter!=otherSignatures.end(); ++iter)
+				 {
+					 ids.insert(iter->first);
+				 }
+			 }
+			 else
+			 {
+				 ids = idsInput;
+			 }
+
+			 const char * colorG = "green";
+			 const char * colorP = "pink";
+	;		 UINFO("Generating map with %d locations", ids.size());
+			 fprintf(fout, "digraph G {\n");
+			 for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
+			 {
+				 if(otherSignatures.find(*i) == otherSignatures.end())
+				 {
+					 int id = *i;
+					 std::map<int, Link> links;
+					 this->loadLinks(id, links);
+					 int weight = 0;
+					 this->getWeight(id, weight);
+					 for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
+					 {
+						 int weightNeighbor = 0;
+						 if(otherSignatures.find(iter->first) == otherSignatures.end())
+						 {
+							 this->getWeight(iter->first, weightNeighbor);
+						 }
+						 else
+						 {
+							 weightNeighbor = otherSignatures.find(iter->first)->second->getWeight();
+						 }
+						 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+						 if(iter->second.type() == Link::kNeighbor)
+						 {
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor);
+						 }
+						 else if(iter->first > id)
+						 {
+							 //loop
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorG);
+						 }
+						 else
+						 {
+							 //child
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorP);
+						 }
+					 }
+				 }
+			 }
+			 for(std::map<int, Signature*>::const_iterator i=otherSignatures.begin(); i!=otherSignatures.end(); ++i)
+			 {
+				 if(ids.find(i->first) != ids.end())
+				 {
+					 int id = i->second->id();
+					 const std::map<int, Link> & links = i->second->getLinks();
+					 int weight = i->second->getWeight();
+					 for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+					 {
+						 int weightNeighbor = 0;
+						 const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0);
+						 if(s)
+						 {
+							 weightNeighbor = s->getWeight();
+						 }
+						 else
+						 {
+							 this->getWeight(iter->first, weightNeighbor);
+						 }
+						 //UDEBUG("Add neighbor link from %d to %d", id, iter->first);
+						 if(iter->second.type() == Link::kNeighbor)
+						 {
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\"\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor);
+						 }
+						 else if(iter->first > id)
+						 {
+							 //loop
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorG);
+						 }
+						 else
+						 {
+							 //child
+							 fprintf(fout, "   \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n",
+									 id,
+									 weight,
+									 iter->first,
+									 weightNeighbor,
+									 colorP);
+						 }
+					 }
+				 }
+			 }
+			 fprintf(fout, "}\n");
+			 fclose(fout);
+			 UINFO("Graph saved to \"%s\" (Tip: $ neato -Tpdf \"%s\" -o out.pdf)", fileName.c_str(), fileName.c_str());
+		}
+	}
+}
+
 } // namespace rtabmap

corelib/src/DBDriverSqlite3.h

@@ -53,6 +53,7 @@ private:
 	virtual void disconnectDatabaseQuery();
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
+	virtual bool getDatabaseVersionQuery(std::string & version) const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
@@ -60,41 +61,34 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
+
+	virtual void addLinkQuery(const Link & link) const;
+	virtual void updateLinkQuery(const Link & link) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
-	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
-	virtual void loadLoopClosuresQuery(
-			int signatureId,
-			std::map<int, Transform> & loopIds,
-			std::map<int, Transform> & childIds) const;
+	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
-	virtual void getNodeDataQuery(
-			int signatureId,
-			cv::Mat & imageCompressed,
-			cv::Mat & depthCompressed,
-			cv::Mat & depth2dCompressed,
-			float & fx,
-			float & fy,
-			float & cx,
-			float & cy,
-			Transform & localTransform) const;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
-	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
 private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepSensorData() const;
+	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
@@ -103,24 +97,15 @@ private:
 			sqlite3_stmt * ppStmt,
 			int id,
 			const cv::Mat & imageBytes) const;
-	void stepDepth(
-			sqlite3_stmt * ppStmt,
-			int id,
-			const cv::Mat & depthBytes,
-			const cv::Mat & depth2dBytes,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			const Transform & localTransform) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
+	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
-	bool getVersion(std::string &) const;
 
 private:
 	sqlite3 * _ppDb;

corelib/src/DBReader.cpp

@@ -31,25 +31,47 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 
 #include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 
 namespace rtabmap {
 
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
-				   float delayToStartSec) :
-	_path(databasePath),
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
+	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
-	_delayToStartSec(delayToStartSec),
+	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
 	_dbDriver(0),
-	_currentId(_ids.end())
+	_currentId(_ids.end()),
+	_previousStamp(0)
 {
+}
 
+DBReader::DBReader(const std::list<std::string> & databasePaths,
+				   float frameRate,
+				   bool odometryIgnored,
+				   bool ignoreGoalDelay,
+				   bool goalsIgnored) :
+   _paths(databasePaths),
+   _frameRate(frameRate),
+	_odometryIgnored(odometryIgnored),
+	_ignoreGoalDelay(ignoreGoalDelay),
+	_goalsIgnored(goalsIgnored),
+	_dbDriver(0),
+	_currentId(_ids.end()),
+	_previousStamp(0)
+{
 }
 
 DBReader::~DBReader()
@@ -71,10 +93,18 @@ bool DBReader::init(int startIndex)
 	}
 	_ids.clear();
 	_currentId=_ids.end();
+	_previousStamp = 0;
 
-	if(!UFile::exists(_path))
+	if(_paths.size() == 0)
 	{
-		UERROR("Database path does not exist (%s)", _path.c_str());
+		UERROR("No database path set...");
+		return false;
+	}
+
+	std::string path = _paths.front();
+	if(!UFile::exists(path))
+	{
+		UERROR("Database path does not exist (%s)", path.c_str());
 		return false;
 	}
 
@@ -86,9 +116,9 @@ bool DBReader::init(int startIndex)
 		UERROR("Driver doesn't exist.");
 		return false;
 	}
-	if(!_dbDriver->openConnection(_path))
+	if(!_dbDriver->openConnection(path))
 	{
-		UERROR("Can't open database %s", _path.c_str());
+		UERROR("Can't open database %s", path.c_str());
 		delete _dbDriver;
 		_dbDriver = 0;
 		return false;
@@ -98,10 +128,10 @@ bool DBReader::init(int startIndex)
 	_currentId = _ids.begin();
 	if(startIndex>0 && _ids.size())
 	{
-		std::set<int>::iterator iter = _ids.lower_bound(startIndex);
+		std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
 		if(iter == _ids.end())
 		{
-			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, *_ids.rbegin());
+			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
 		}
 		else
 		{
@@ -114,130 +144,224 @@ bool DBReader::init(int startIndex)
 
 void DBReader::setFrameRate(float frameRate)
 {
-	if(frameRate >= 0.0f)
-	{
-		_frameRate = frameRate;
-	}
+	_frameRate = frameRate;
 }
 
 void DBReader::mainLoopBegin()
 {
-	if(_delayToStartSec > 0.0f)
-	{
-		uSleep(_delayToStartSec*1000.0f);
-	}
 	_timer.start();
 }
 
 void DBReader::mainLoop()
 {
-	cv::Mat image, depth, depth2d;
-	float fx,fy,cx,cy;
-	Transform localTransform, pose;
-	int seq = 0;
-	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
-	if(!image.empty())
+	OdometryEvent odom = this->getNextData();
+	if(odom.data().id())
 	{
-		if(depth.empty())
+		int goalId = 0;
+		double previousStamp = odom.data().stamp();
+		if(previousStamp == 0)
 		{
-			this->post(new CameraEvent(image));
+			odom.data().setStamp(UTimer::now());
+		}
+		if(!_goalsIgnored &&
+		   odom.data().userDataRaw().type() == CV_8SC1 &&
+		   odom.data().userDataRaw().cols >= 7 && // including null str ending
+		   odom.data().userDataRaw().rows == 1 &&
+		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = (const char *)odom.data().userDataRaw().data;
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					goalId = atoi(strs.rbegin()->c_str());
+					odom.data().setUserData(cv::Mat());
+				}
+			}
+		}
+		if(!_odometryIgnored)
+		{
+			if(odom.pose().isNull())
+			{
+				UWARN("Reading the database: odometry is null! "
+					  "Please set \"Ignore odometry = true\" if there is "
+					  "no odometry in the database.");
+			}
+			this->post(new OdometryEvent(odom));
 		}
 		else
 		{
-			if(!_odometryIgnored)
+			this->post(new CameraEvent(odom.data()));
+		}
+
+		if(goalId > 0)
+		{
+			if(!_ignoreGoalDelay && _currentId != _ids.end())
 			{
-				SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
-				this->post(new OdometryEvent(data));
-				if(pose.isNull())
+				// get stamp for the next signature to compute the delay
+				// that was used originally for planning
+				int weight;
+				std::string label;
+				double stamp;
+				int mapId;
+				Transform localTransform, pose;
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
+				if(previousStamp && stamp && stamp > previousStamp)
 				{
-					UWARN("Reading the database: odometry is null! "
-						  "Please set \"Ignore odometry = true\" if there is "
-						  "no odometry in the database.");
+					double delay = stamp - previousStamp;
+					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
+							goalId, delay);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
+					uSleep(delay*1000);
+				}
+				else
+				{
+					UWARN("Goal %d detected, posting it!", goalId);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
 				}
 			}
 			else
 			{
-				// without odometry
-				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
+				UWARN("Goal %d detected, posting it!", goalId);
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
 			}
 		}
+
 	}
 	else if(!this->isKilled())
 	{
 		UINFO("no more images...");
-		this->kill();
-		this->post(new CameraEvent());
+		if(_paths.size() > 1)
+		{
+			_paths.pop_front();
+			UWARN("Loading next database \"%s\"...", _paths.front().c_str());
+			if(!this->init())
+			{
+				UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
+				this->kill();
+				this->post(new CameraEvent());
+			}
+		}
+		else
+		{
+			this->kill();
+			this->post(new CameraEvent());
+		}
+
 	}
 
 }
 
-void DBReader::getNextImage(
-		cv::Mat & image,
-		cv::Mat & depth,
-		cv::Mat & depth2d,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		Transform & pose,
-		int & seq)
+OdometryEvent DBReader::getNextData()
 {
+	OdometryEvent odom;
 	if(_dbDriver)
 	{
-		float frameRate = _frameRate;
-		if(frameRate>0.0f)
-		{
-			int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
-			if(sleepTime > 2)
-			{
-				uSleep(sleepTime-2);
-			}
-
-			// Add precision at the cost of a small overhead
-			while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
-			{
-				//
-			}
-
-			double slept = _timer.getElapsedTime();
-			_timer.start();
-			UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
-		}
-
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
-			cv::Mat imageBytes;
-			cv::Mat depthBytes;
-			cv::Mat depth2dBytes;
 			int mapId;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
-			_dbDriver->getPose(*_currentId, pose, mapId);
-			seq = *_currentId;
+			SensorData data;
+			_dbDriver->getNodeData(*_currentId, data);
+
+			// info
+			Transform pose;
+			int weight;
+			std::string label;
+			double stamp;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
+
+			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+			if(!_odometryIgnored)
+			{
+				std::map<int, Link> links;
+				_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
+				if(links.size())
+				{
+					// assume the first is the backward neighbor, take its variance
+					infMatrix = links.begin()->second.infMatrix();
+				}
+			}
+			else
+			{
+				pose.setNull();
+			}
+
+			int seq = *_currentId;
 			++_currentId;
-			if(imageBytes.empty())
+			if(data.imageCompressed().empty())
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
 
-			util3d::CompressionThread ctImage(imageBytes, true);
-			util3d::CompressionThread ctDepth(depthBytes, true);
-			util3d::CompressionThread ctDepth2D(depth2dBytes, false);
-			ctImage.start();
-			ctDepth.start();
-			ctDepth2D.start();
-			ctImage.join();
-			ctDepth.join();
-			ctDepth2D.join();
-			image = ctImage.getUncompressedData();
-			depth = ctDepth.getUncompressedData();
-			depth2d = ctDepth2D.getUncompressedData();
+			// Frame rate
+			if(_frameRate < 0.0f)
+			{
+				if(stamp == 0)
+				{
+					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
+					this->kill();
+				}
+				else if(_previousStamp > 0)
+				{
+					int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
+					if(sleepTime > 2)
+					{
+						uSleep(sleepTime-2);
+					}
+
+					// Add precision at the cost of a small overhead
+					while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
+					{
+						//
+					}
+
+					double slept = _timer.getElapsedTime();
+					_timer.start();
+					UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
+				}
+				_previousStamp = stamp;
+			}
+			else if(_frameRate>0.0f)
+			{
+				int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
+				if(sleepTime > 2)
+				{
+					uSleep(sleepTime-2);
+				}
+
+				// Add precision at the cost of a small overhead
+				while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
+				{
+					//
+				}
+
+				double slept = _timer.getElapsedTime();
+				_timer.start();
+				UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
+			}
+
+			if(!this->isKilled())
+			{
+				data.uncompressData();
+				data.setId(seq);
+				data.setStamp(stamp);
+				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
+						data.laserScanRaw().empty()?0:1,
+						data.imageRaw().empty()?0:1,
+						data.depthOrRightRaw().empty()?0:1,
+						data.userDataRaw().empty()?0:1);
+
+				odom = OdometryEvent(data, pose, infMatrix.inv());
+			}
 		}
 	}
 	else
 	{
 		UERROR("Not initialized...");
 	}
+	return odom;
 }
 
 } /* namespace rtabmap */

corelib/src/EpipolarGeometry.cpp

@@ -151,13 +151,18 @@ int inFrontOfBothCameras(const cv::Mat & x, const cv::Mat & xp, const cv::Mat &
 	p.at<double>(2,3) = T.at<double>(2,0);
 
 	cv::Mat pts4D;
+	//std::vector<double> reprojErrors;
+	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+	//EpipolarGeometry::triangulatePoints(x, xp, p0, p, cloud, reprojErrors);
 	cv::triangulatePoints(p0, p, x, xp, pts4D);
 
     //http://en.wikipedia.org/wiki/Essential_matrix#3D_points_from_corresponding_image_points
 	int nValid = 0;
     for(int i=0; i<x.cols; ++i)
     {
+    	// the five to ignore when all points are super close to the camera
         if(pts4D.at<double>(2,i)/pts4D.at<double>(3,i) > 5)
+    	//if(cloud->at(i).z > 5)
         {
         	++nValid;
         }
@@ -210,7 +215,7 @@ cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
 	cv::Mat r = u*w*vt;
 	if(cv::determinant(r)+1.0 < 1e-09) {
 		//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
-		UWARN("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
+		UDEBUG("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
 		e = -E;
 		svd(e,cv::SVD::MODIFY_A);
 		u = svd.u;
@@ -373,14 +378,10 @@ void EpipolarGeometry::findRTFromP(
 		cv::Mat & t)
 {
 	UASSERT(p.cols == 4 && p.rows == 3);
-	UDEBUG("");
 	r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
-	UDEBUG("");
 	//r = -r.inv();
-	UDEBUG("r=%d %d, t=%d", r.cols, r.rows, p.col(3).rows);
 	//t = r*p.col(3);
 	t = p.col(3);
-	UDEBUG("");
 }
 
 cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)

corelib/src/Features2d.cpp

@@ -34,14 +34,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include <opencv2/imgproc/imgproc_c.h>
-#include <opencv2/gpu/gpu.hpp>
 #include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>
 
-#ifdef WITH_NONFREE
-#if CV_MAJOR_VERSION > 2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-#include <opencv2/nonfree/gpu.hpp>
-#include <opencv2/nonfree/features2d.hpp>
+#if CV_MAJOR_VERSION < 3
+#include <opencv2/gpu/gpu.hpp>
+#else
+#include <opencv2/core/cuda.hpp>
 #endif
+
+#ifdef HAVE_OPENCV_NONFREE
+  #if CV_MAJOR_VERSION == 2 && CV_MINOR_VERSION >=4
+  #include <opencv2/nonfree/gpu.hpp>
+  #include <opencv2/nonfree/features2d.hpp>
+  #endif
+#endif
+#ifdef HAVE_OPENCV_XFEATURES2D
+  #include <opencv2/xfeatures2d.hpp>
+  #include <opencv2/xfeatures2d/nonfree.hpp>
+  #include <opencv2/xfeatures2d/cuda.hpp>
 #endif
 
 namespace rtabmap {
@@ -251,7 +262,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRat
 		unsigned int i=0;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			values[i] = std::atof((*iter).c_str());
+			values[i] = uStr2Float(*iter);
 			++i;
 		}
 
@@ -319,14 +330,40 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 /////////////////////
 // Feature2D
 /////////////////////
+Feature2D::Feature2D(const ParametersMap & parameters) :
+		maxFeatures_(Parameters::defaultKpWordsPerImage())
+{
+	this->parseParameters(parameters);
+}
+void Feature2D::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
+}
 Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
 {
-	if(RTABMAP_NONFREE == 0 &&
-	   (type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
+	if(RTABMAP_NONFREE == 0)
 	{
-		UERROR("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
-		type = Feature2D::kFeatureOrb;
+		if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
+		{
+#if CV_MAJOR_VERSION < 3
+			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead.");
+#else
+			UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
+#endif
+			type = Feature2D::kFeatureOrb;
+		}
+#if CV_MAJOR_VERSION == 3
+		if(type == Feature2D::kFeatureFastBrief ||
+		   type == Feature2D::kFeatureFastFreak ||
+		   type == Feature2D::kFeatureGfttBrief ||
+		   type == Feature2D::kFeatureGfttFreak)
+		{
+			UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
+			type = Feature2D::kFeatureOrb;
+		}
+#endif
 	}
+
 	Feature2D * feature2D = 0;
 	switch(type)
 	{
@@ -354,7 +391,7 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 	case Feature2D::kFeatureBrisk:
 		feature2D = new BRISK(parameters);
 		break;
-#ifdef WITH_NONFREE
+#if RTABMAP_NONFREE == 1
 	default:
 		feature2D = new SURF(parameters);
 		type = Feature2D::kFeatureSurf;
@@ -369,9 +406,8 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 	}
 	return feature2D;
 }
-std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
+std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
 {
-	ULOGGER_DEBUG("");
 	std::vector<cv::KeyPoint> keypoints;
 	if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
 	{
@@ -381,7 +417,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, in
 		keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
 		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
 
-		limitKeypoints(keypoints, maxKeypoints);
+		limitKeypoints(keypoints, maxFeatures_);
 
 		if(roi.x || roi.y)
 		{
@@ -424,29 +460,19 @@ SURF::SURF(const ParametersMap & parameters) :
 		extended_(Parameters::defaultSURFExtended()),
 		upright_(Parameters::defaultSURFUpright()),
 		gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
-		gpuVersion_(Parameters::defaultSURFGpuVersion()),
-		_surf(0),
-		_gpuSurf(0)
+		gpuVersion_(Parameters::defaultSURFGpuVersion())
 {
 	parseParameters(parameters);
 }
 
 SURF::~SURF()
 {
-#ifdef WITH_NONFREE
-	if(_surf)
-	{
-		delete _surf;
-	}
-	if(_gpuSurf)
-	{
-		delete _gpuSurf;
-	}
-#endif
 }
 
 void SURF::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
 	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
 	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
@@ -455,33 +481,34 @@ void SURF::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
 	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
 
-#ifdef WITH_NONFREE
-	if(_gpuSurf)
+#if RTABMAP_NONFREE == 1
+#if CV_MAJOR_VERSION < 3
+	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuSurf;
-		_gpuSurf = 0;
+		UWARN("GPU version of SURF not available! Using CPU version instead...");
+		gpuVersion_ = false;
 	}
-	if(_surf)
+#else
+	if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _surf;
-		_surf = 0;
+		UWARN("GPU version of SURF not available! Using CPU version instead...");
+		gpuVersion_ = false;
 	}
-
-	if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount())
+#endif
+	if(gpuVersion_)
 	{
-		_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_);
+		_gpuSurf = cv::Ptr<CV_SURF_GPU>(new CV_SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_));
 	}
 	else
 	{
-		if(gpuVersion_)
-		{
-			UWARN("GPU version of SURF not available! Using CPU version instead...");
-		}
-
-		_surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
+#if CV_MAJOR_VERSION < 3
+		_surf = cv::Ptr<CV_SURF>(new CV_SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_));
+#else
+		_surf = CV_SURF::create(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
+#endif
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 }
 
@@ -490,19 +517,24 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 
-#ifdef WITH_NONFREE
+#if RTABMAP_NONFREE == 1
 	cv::Mat imgRoi(image, roi);
-	if(_gpuSurf)
+	if(gpuVersion_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+		cv::cuda::GpuMat imgGpu(imgRoi);
+		(*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints);
+#endif
 	}
 	else
 	{
 		_surf->detect(imgRoi, keypoints);
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 	return keypoints;
 }
@@ -511,12 +543,18 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#ifdef WITH_NONFREE
-	if(_gpuSurf)
+#if RTABMAP_NONFREE == 1
+	if(gpuVersion_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(image);
 		cv::gpu::GpuMat descriptorsGPU;
-		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
+		(*_gpuSurf.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
+#else
+		cv::cuda::GpuMat imgGpu(image);
+		cv::cuda::GpuMat descriptorsGPU;
+		(*_gpuSurf.get())(imgGpu, cv::cuda::GpuMat(), keypoints, descriptorsGPU, true);
+#endif
 
 		// Download descriptors
 		if (descriptorsGPU.empty())
@@ -533,7 +571,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 		_surf->compute(image, keypoints, descriptors);
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 
 	return descriptors;
@@ -547,42 +585,33 @@ SIFT::SIFT(const ParametersMap & parameters) :
 	nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
 	contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
 	edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
-	sigma_(Parameters::defaultSIFTSigma()),
-	_sift(0)
+	sigma_(Parameters::defaultSIFTSigma())
 {
 	parseParameters(parameters);
 }
 
 SIFT::~SIFT()
 {
-#ifdef WITH_NONFREE
-	if(_sift)
-	{
-		delete _sift;
-	}
-#else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
-#endif
 }
 
 void SIFT::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
 	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
 
-#ifdef WITH_NONFREE
-	if(_sift)
-	{
-		delete _sift;
-		_sift = 0;
-	}
-
-	_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#if RTABMAP_NONFREE == 1
+#if CV_MAJOR_VERSION < 3
+	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	_sift = CV_SIFT::create(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#endif
+#else
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 }
 
@@ -590,11 +619,11 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef WITH_NONFREE
+#if RTABMAP_NONFREE == 1
 	cv::Mat imgRoi(image, roi);
 	_sift->detect(imgRoi, keypoints); // Opencv keypoints
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 	return keypoints;
 }
@@ -603,10 +632,10 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#ifdef WITH_NONFREE
+#if RTABMAP_NONFREE == 1
 	_sift->compute(image, keypoints, descriptors);
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 	return descriptors;
 }
@@ -615,7 +644,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 //ORB
 //////////////////////////
 ORB::ORB(const ParametersMap & parameters) :
-		nFeatures_(Parameters::defaultORBNFeatures()),
+		nFeatures_(Parameters::defaultKpWordsPerImage()),
 		scaleFactor_(Parameters::defaultORBScaleFactor()),
 		nLevels_(Parameters::defaultORBNLevels()),
 		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
@@ -625,28 +654,20 @@ ORB::ORB(const ParametersMap & parameters) :
 		patchSize_(Parameters::defaultORBPatchSize()),
 		gpu_(Parameters::defaultORBGpu()),
 		fastThreshold_(Parameters::defaultFASTThreshold()),
-		nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression()),
-		_orb(0),
-		_gpuOrb(0)
+		nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression())
 {
 	parseParameters(parameters);
 }
 
 ORB::~ORB()
 {
-	if(_orb)
-	{
-		delete _orb;
-	}
-	if(_gpuOrb)
-	{
-		delete _gpuOrb;
-	}
 }
 
 void ORB::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures_);
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
 	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
 	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
 	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
@@ -659,29 +680,44 @@ void ORB::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
 	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);
 
-	if(_gpuOrb)
+#if CV_MAJOR_VERSION < 3
+	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuOrb;
-		_gpuOrb = 0;
+		UWARN("GPU version of ORB not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-	if(_orb)
+#else
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+	if(gpu_)
 	{
-		delete _orb;
-		_orb = 0;
+		UWARN("GPU version of ORB not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
+		gpu_ = false;
 	}
-
-	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
+#endif
+	if(gpu_)
 	{
-		_gpuOrb = new cv::gpu::ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+		UWARN("GPU version of ORB available but not implemented yet! Using CPU version instead...");
+	}
+	gpu_ = false;
+#endif
+	if(gpu_)
+	{
+#if CV_MAJOR_VERSION < 3
+		_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
 		_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
-		if(gpu_)
-		{
-			UWARN("GPU version of ORB not available! Using CPU version instead...");
-		}
-		_orb = new cv::ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+#if CV_MAJOR_VERSION < 3
+		_orb = cv::Ptr<CV_ORB>(new CV_ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
+#else
+		_orb = CV_ORB::create(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
+#endif
 	}
 }
 
@@ -690,10 +726,17 @@ std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, cons
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat imgRoi(image, roi);
-	if(_gpuOrb)
+
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
@@ -712,11 +755,19 @@ cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyP
 		ULOGGER_ERROR("Image is null ?!?");
 		return descriptors;
 	}
-	if(_gpuOrb)
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(image);
 		cv::gpu::GpuMat descriptorsGPU;
-		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
+		(*_gpuOrb.obj)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
+#else
+		cv::cuda::GpuMat imgGpu(image);
+		cv::cuda::GpuMat descriptorsGPU;
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 
 		// Download descriptors
 		if (descriptorsGPU.empty())
@@ -743,54 +794,66 @@ FAST::FAST(const ParametersMap & parameters) :
 		threshold_(Parameters::defaultFASTThreshold()),
 		nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
 		gpu_(Parameters::defaultFASTGpu()),
-		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
-		_fast(0),
-		_gpuFast(0)
+		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio())
 {
 	parseParameters(parameters);
 }
 
 FAST::~FAST()
 {
-	if(_fast)
-	{
-		delete _fast;
-	}
-	if(_gpuFast)
-	{
-		delete _gpuFast;
-	}
 }
 
 void FAST::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
 	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
 	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
 	Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);
 
-	if(_gpuFast)
+#if CV_MAJOR_VERSION < 3
+	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _gpuFast;
-		_gpuFast = 0;
+		UWARN("GPU version of FAST not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-	if(_fast)
+#else
+	if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
 	{
-		delete _fast;
-		_fast = 0;
+		UWARN("GPU version of FAST not available! Using CPU version instead...");
+		gpu_ = false;
 	}
-
-	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+	if(gpu_)
 	{
-		_gpuFast = new cv::gpu::FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
+		UWARN("GPU version of FAST not available (OpenCV cudafeatures2d module)! Using CPU version instead...");
+		gpu_ = false;
+	}
+#endif
+	if(gpu_)
+	{
+		UWARN("GPU version of FAST is available but not yet implemented! Using CPU version instead...");
+	}
+	gpu_ = false;
+#endif
+	if(gpu_)
+	{
+#if CV_MAJOR_VERSION < 3
+		_gpuFast = new CV_FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
-		if(gpu_)
-		{
-			UWARN("GPU version of FAST not available! Using CPU version instead...");
-		}
-		_fast = new cv::FastFeatureDetector(threshold_, nonmaxSuppression_);
+#if CV_MAJOR_VERSION < 3
+		_fast = cv::Ptr<CV_FAST>(new CV_FAST(threshold_, nonmaxSuppression_));
+#else
+		_fast = CV_FAST::create(threshold_, nonmaxSuppression_);
+#endif
 	}
 }
 
@@ -799,10 +862,16 @@ std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, con
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat imgRoi(image, roi);
-	if(_gpuFast)
+	if(gpu_)
 	{
+#if CV_MAJOR_VERSION < 3
 		cv::gpu::GpuMat imgGpu(imgRoi);
-		(*_gpuFast)(imgGpu, cv::gpu::GpuMat(), keypoints);
+		(*_gpuFast.obj)(imgGpu, cv::gpu::GpuMat(), keypoints);
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+		UFATAL("not implemented");
+#endif
+#endif
 	}
 	else
 	{
@@ -816,18 +885,13 @@ std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, con
 //////////////////////////
 FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
 	FAST(parameters),
-	bytes_(Parameters::defaultBRIEFBytes()),
-	_brief(0)
+	bytes_(Parameters::defaultBRIEFBytes())
 {
 	parseParameters(parameters);
 }
 
 FAST_BRIEF::~FAST_BRIEF()
 {
-	if(_brief)
-	{
-		delete _brief;
-	}
 }
 
 void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
@@ -835,19 +899,30 @@ void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
 	FAST::parseParameters(parameters);
 
 	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
-	if(_brief)
-	{
-		delete _brief;
-		_brief = 0;
-	}
-	_brief = new cv::BriefDescriptorExtractor(bytes_);
+#if CV_MAJOR_VERSION < 3
+	_brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief = CV_BRIEF::create(bytes_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 }
 
 cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_brief->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 	return descriptors;
 }
 
@@ -859,18 +934,13 @@ FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
 	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
 	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
 	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves()),
-	_freak(0)
+	nOctaves_(Parameters::defaultFREAKNOctaves())
 {
 	parseParameters(parameters);
 }
 
 FAST_FREAK::~FAST_FREAK()
 {
-	if(_freak)
-	{
-		delete _freak;
-	}
 }
 
 void FAST_FREAK::parseParameters(const ParametersMap & parameters)
@@ -882,20 +952,30 @@ void FAST_FREAK::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
 	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
 
-	if(_freak)
-	{
-		delete _freak;
-		_freak = 0;
-	}
-
-	_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 }
 
 cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 	return descriptors;
 }
 
@@ -903,40 +983,36 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 //GFTT
 //////////////////////////
 GFTT::GFTT(const ParametersMap & parameters) :
-		_maxCorners(Parameters::defaultGFTTMaxCorners()),
+		_maxCorners(Parameters::defaultKpWordsPerImage()),
 		_qualityLevel(Parameters::defaultGFTTQualityLevel()),
 		_minDistance(Parameters::defaultGFTTMinDistance()),
 		_blockSize(Parameters::defaultGFTTBlockSize()),
 		_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
-		_k(Parameters::defaultGFTTK()),
-		_gftt(0)
+		_k(Parameters::defaultGFTTK())
 {
 	parseParameters(parameters);
 }
 
 GFTT::~GFTT()
 {
-	if(_gftt)
-	{
-		delete _gftt;
-	}
 }
 
 void GFTT::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kGFTTMaxCorners(), _maxCorners);
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
 	Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
 	Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
 	Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
 	Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
 	Parameters::parse(parameters, Parameters::kGFTTK(), _k);
 
-	if(_gftt)
-	{
-		delete _gftt;
-		_gftt = 0;
-	}
-	_gftt = new cv::GFTTDetector(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#if CV_MAJOR_VERSION < 3
+	_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
+#else
+	_gftt = CV_GFTT::create(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+#endif
 }
 
 std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
@@ -953,18 +1029,13 @@ std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, con
 //////////////////////////
 GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
 	GFTT(parameters),
-	bytes_(Parameters::defaultBRIEFBytes()),
-	_brief(0)
+	bytes_(Parameters::defaultBRIEFBytes())
 {
 	parseParameters(parameters);
 }
 
 GFTT_BRIEF::~GFTT_BRIEF()
 {
-	if(_brief)
-	{
-		delete _brief;
-	}
 }
 
 void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
@@ -972,19 +1043,30 @@ void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
 	GFTT::parseParameters(parameters);
 
 	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
-	if(_brief)
-	{
-		delete _brief;
-		_brief = 0;
-	}
-	_brief = new cv::BriefDescriptorExtractor(bytes_);
+#if CV_MAJOR_VERSION < 3
+	_brief = cv::Ptr<CV_BRIEF>(new CV_BRIEF(bytes_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief = CV_BRIEF::create(bytes_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 }
 
 cv::Mat GFTT_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_brief->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_brief->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Brief cannot be used!");
+#endif
+#endif
 	return descriptors;
 }
 
@@ -996,18 +1078,13 @@ GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
 	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
 	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
 	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves()),
-	_freak(0)
+	nOctaves_(Parameters::defaultFREAKNOctaves())
 {
 	parseParameters(parameters);
 }
 
 GFTT_FREAK::~GFTT_FREAK()
 {
-	if(_freak)
-	{
-		delete _freak;
-	}
 }
 
 void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
@@ -1019,20 +1096,30 @@ void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
 	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
 
-	if(_freak)
-	{
-		delete _freak;
-		_freak = 0;
-	}
-
-	_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#if CV_MAJOR_VERSION < 3
+	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 }
 
 cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
+#if CV_MAJOR_VERSION < 3
 	_freak->compute(image, keypoints, descriptors);
+#else
+#ifdef HAVE_OPENCV_XFEATURES2D
+	_freak->compute(image, keypoints, descriptors);
+#else
+	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
+#endif
+#endif
 	return descriptors;
 }
 
@@ -1042,33 +1129,28 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 BRISK::BRISK(const ParametersMap & parameters) :
 	thresh_(Parameters::defaultBRISKThresh()),
 	octaves_(Parameters::defaultBRISKOctaves()),
-	patternScale_(Parameters::defaultBRISKPatternScale()),
-	brisk_(0)
+	patternScale_(Parameters::defaultBRISKPatternScale())
 {
 	parseParameters(parameters);
 }
 
 BRISK::~BRISK()
 {
-	if(brisk_)
-	{
-		delete brisk_;
-	}
 }
 
 void BRISK::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
 	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
 	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
 
-	if(brisk_)
-	{
-		delete brisk_;
-		brisk_ = 0;
-	}
-
-	brisk_ = new cv::BRISK(thresh_, octaves_, patternScale_);
+#if CV_MAJOR_VERSION < 3
+	brisk_ = cv::Ptr<CV_BRISK>(new CV_BRISK(thresh_, octaves_, patternScale_));
+#else
+	brisk_ = CV_BRISK::create(thresh_, octaves_, patternScale_);
+#endif
 }
 
 std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const

corelib/src/Link.cpp

@@ -0,0 +1,198 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Link.h"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/core/Compression.h>
+
+namespace rtabmap {
+
+Link::Link() :
+	from_(0),
+	to_(0),
+	type_(kUndef),
+	infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
+{
+}
+Link::Link(int from,
+		int to,
+		Type type,
+		const Transform & transform,
+		const cv::Mat & infMatrix,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setInfMatrix(infMatrix);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+Link::Link(int from,
+		int to,
+		Type type,
+		const Transform & transform,
+		double rotVariance,
+		double transVariance,
+		const cv::Mat & userData) :
+	from_(from),
+	to_(to),
+	transform_(transform),
+	type_(type)
+{
+	setVariance(rotVariance, transVariance);
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+double Link::rotVariance() const
+{
+	double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
+	UASSERT(min > 0.0);
+	return 1.0/min;
+}
+double Link::transVariance() const
+{
+	double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
+	UASSERT(min > 0.0);
+	return 1.0/min;
+}
+
+void Link::setInfMatrix(const cv::Mat & infMatrix) {
+	UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)");
+	infMatrix_ = infMatrix;
+}
+void Link::setVariance(double rotVariance, double transVariance) {
+	UASSERT(uIsFinite(rotVariance) && rotVariance>0);
+	UASSERT(uIsFinite(transVariance) && transVariance>0);
+	infMatrix_ = cv::Mat::eye(6,6,CV_64FC1);
+	infMatrix_.at<double>(0,0) = 1.0/transVariance;
+	infMatrix_.at<double>(1,1) = 1.0/transVariance;
+	infMatrix_.at<double>(2,2) = 1.0/transVariance;
+	infMatrix_.at<double>(3,3) = 1.0/rotVariance;
+	infMatrix_.at<double>(4,4) = 1.0/rotVariance;
+	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
+}
+
+void Link::setUserDataRaw(const cv::Mat & userDataRaw)
+{
+	if(!_userDataRaw.empty())
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = userDataRaw;
+}
+
+void Link::setUserData(const cv::Mat & userData)
+{
+	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = cv::Mat();
+	_userDataCompressed = cv::Mat();
+
+	if(!userData.empty())
+	{
+		if(userData.type() == CV_8UC1) // Bytes
+		{
+			_userDataCompressed = userData; // assume compressed
+		}
+		else
+		{
+			_userDataRaw = userData;
+			_userDataCompressed = compressData2(userData);
+		}
+	}
+}
+
+void Link::uncompressUserData()
+{
+	cv::Mat dataRaw = uncompressUserDataConst();
+	if(!dataRaw.empty() && _userDataRaw.empty())
+	{
+		_userDataRaw = dataRaw;
+	}
+}
+
+cv::Mat Link::uncompressUserDataConst() const
+{
+	if(!_userDataRaw.empty())
+	{
+		return _userDataRaw;
+	}
+	return uncompressData(_userDataCompressed);
+}
+
+Link Link::merge(const Link & link, Type outputType) const
+{
+	UASSERT(to_ == link.from());
+	UASSERT(outputType != Link::kUndef);
+	UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
+	UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
+	UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
+	return Link(
+			from_,
+			link.to(),
+			outputType,
+			transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix());
+}
+
+Link Link::inverse() const
+{
+	return Link(
+			to_,
+			from_,
+			type_,
+			transform_.isNull()?Transform():transform_.inverse(),
+			transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
+}
+
+}

corelib/src/OdometryBOW.cpp

@@ -0,0 +1,439 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_correspondences.h"
+#include "rtabmap/core/util3d_motion_estimation.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/VWDictionary.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/calib3d/calib3d.hpp>
+
+#if _MSC_VER
+	#define ISFINITE(value) _finite(value)
+#else
+	#define ISFINITE(value) std::isfinite(value)
+#endif
+
+namespace rtabmap {
+
+OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
+	Odometry(parameters),
+	_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
+	_fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()),
+	_memory(0)
+{
+	UDEBUG("");
+	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
+
+	ParametersMap customParameters;
+	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
+	int nn = Parameters::defaultOdomBowNNType();
+	float nndr = Parameters::defaultOdomBowNNDR();
+	int featureType = Parameters::defaultOdomFeatureType();
+	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
+	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
+
+	// Memory's stereo parameters, copy from Odometry
+	int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
+	int subPixIterations = Parameters::defaultOdomSubPixIterations();
+	double subPixEps = Parameters::defaultOdomSubPixEps();
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
+
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	if(_fixedLocalMapPath.empty())
+	{
+		_memory = new Memory(customParameters);
+		if(!_memory->init("", false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+	}
+	else
+	{
+		UINFO("Init odometry from a fixed database: \"%s\"", _fixedLocalMapPath.c_str());
+		// init the local map with a all 3D features contained in the database
+		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
+		customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
+		_memory = new Memory(customParameters);
+		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+		else
+		{
+			// get the graph
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, -1);
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> links;
+			_memory->getMetricConstraints(uKeysSet(ids), poses, links, true);
+
+			if(poses.size())
+			{
+				//optimize the graph
+				graph::TOROOptimizer optimizer;
+				std::map<int, Transform> optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links);
+
+				// fill the local map
+				for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
+					posesIter!=optimizedPoses.end();
+					++posesIter)
+				{
+					const Signature * s = _memory->getSignature(posesIter->first);
+					if(s)
+					{
+						// Transform 3D points accordingly to pose and add them to local map
+						const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
+						for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
+							pointsIter!=words3D.end();
+							++pointsIter)
+						{
+							if(!uContains(localMap_, pointsIter->first))
+							{
+								localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
+							}
+						}
+					}
+				}
+			}
+			else
+			{
+				UERROR("No pose loaded from database \"%s\"", _fixedLocalMapPath.c_str());
+			}
+		}
+		if((int)localMap_.size() < this->getMinInliers() || localMap_.size() == 0)
+		{
+			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
+					_fixedLocalMapPath.c_str(), (int)localMap_.size());
+		}
+	}
+}
+
+OdometryBOW::~OdometryBOW()
+{
+	delete _memory;
+	UDEBUG("");
+}
+
+
+void OdometryBOW::reset(const Transform & initialPose)
+{
+	if(_fixedLocalMapPath.empty())
+	{
+		Odometry::reset(initialPose);
+		_memory->init("", false, ParametersMap());
+		localMap_.clear();
+	}
+	else
+	{
+		UWARN("Odometry cannot be reset when a fixed local map is set.");
+	}
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryBOW::computeTransform(
+		const SensorData & data,
+		OdometryInfo * info)
+{
+	UTimer timer;
+	Transform output;
+
+	if(info)
+	{
+		info->type = 0;
+	}
+
+	double variance = 0;
+	int inliersCount = 0;
+	int correspondences = 0;
+	int nFeatures = 0;
+
+	if(_memory->update(data))
+	{
+		const Signature * newSignature = _memory->getLastWorkingSignature();
+		if(newSignature)
+		{
+			nFeatures = (int)newSignature->getWords().size();
+			if(this->isInfoDataFilled() && info)
+			{
+				info->words = newSignature->getWords();
+			}
+		}
+
+		if(localMap_.size() && newSignature)
+		{
+			Transform transform;
+			if((int)localMap_.size() >= this->getMinInliers())
+			{
+				std::vector<int> matches, inliers;
+				Transform t;
+				if(this->getEstimationType() == 1) // PnP
+				{
+					// 3D to 2D
+					if(data.cameraModels().size() > 1)
+					{
+						UERROR("PnP cannot be used on multi-cameras setup.");
+					}
+					else if((int)newSignature->getWords().size() >= this->getMinInliers())
+					{
+						UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
+						const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
+
+						UDEBUG("");
+						t = util3d::estimateMotion3DTo2D(
+								localMap_,
+								uMultimapToMap(newSignature->getWords()),
+								cameraModel,
+								this->getMinInliers(),
+								this->getIterations(),
+								this->getPnPReprojError(),
+								this->getPnPFlags(),
+								this->getPose(),
+								uMultimapToMap(newSignature->getWords3()),
+								&variance,
+								&matches,
+								&inliers);
+					}
+					else
+					{
+						UWARN("Not enough features in the new image (%d < %d)", (int)newSignature->getWords().size(), this->getMinInliers());
+					}
+				}
+				else
+				{
+					// 3D to 3D
+					if((int)newSignature->getWords3().size() >= this->getMinInliers())
+					{
+						t = util3d::estimateMotion3DTo3D(
+								localMap_,
+								uMultimapToMap(newSignature->getWords3()),
+								this->getMinInliers(),
+								this->getInlierDistance(),
+								this->getIterations(),
+								this->getRefineIterations(),
+								&variance,
+								&matches,
+								&inliers);
+					}
+					else
+					{
+						UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
+					}
+				}
+
+				correspondences = matches.size();
+				inliersCount = inliers.size();
+				if(this->isInfoDataFilled() && info)
+				{
+					info->wordMatches = matches;
+					info->wordInliers = inliers;
+				}
+
+				if(!t.isNull())
+				{
+					// make it incremental
+					transform = this->getPose().inverse() * t;
+				}
+				else if(correspondences < this->getMinInliers())
+				{
+					UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+				}
+				else if(inliersCount < this->getMinInliers())
+				{
+					UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers());
+				}
+				else
+				{
+					UWARN("Unknown estimation error");
+				}
+			}
+			else
+			{
+				UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), this->getMinInliers());
+			}
+
+			if(transform.isNull())
+			{
+				_memory->deleteLocation(newSignature->id());
+			}
+			else if(_fixedLocalMapPath.empty())
+			{
+				output = transform;
+
+				// remove words if history max size is reached
+				while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
+				{
+					int nodeId = *_memory->getStMem().begin();
+					std::list<int> removedPts;
+					_memory->deleteLocation(nodeId, &removedPts);
+					for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
+					{
+						localMap_.erase(*iter);
+					}
+				}
+
+				if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
+				{
+					UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
+							(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
+				}
+
+				// update local map
+				std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+				Transform t = this->getPose()*output;
+				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+				{
+					// Only add unique words not in local map
+					if(newSignature->getWords3().count(*iter) == 1)
+					{
+						// keep old word
+						if(localMap_.find(*iter) == localMap_.end())
+						{
+							const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+							if(pcl::isFinite(pt))
+							{
+								pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
+								localMap_.insert(std::make_pair(*iter, pt2));
+							}
+						}
+					}
+					else
+					{
+						localMap_.erase(*iter);
+					}
+				}
+			}
+			else
+			{
+				// fixed local map, just delete the new signature
+				output = transform;
+				_memory->deleteLocation(newSignature->id());
+			}
+		}
+		else if(newSignature)
+		{
+			int count = 0;
+			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+			if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers())
+			{
+				output.setIdentity();
+
+				Transform t = this->getPose(); // initial pose maybe not identity...
+				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+				{
+					// Only add unique words
+					if(newSignature->getWords3().count(*iter) == 1)
+					{
+						const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+						if(pcl::isFinite(pt))
+						{
+							pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
+							localMap_.insert(std::make_pair(*iter, pt2));
+						}
+						else
+						{
+							++count;
+						}
+					}
+				}
+			}
+			else
+			{
+				// not enough features, just delete it
+				_memory->deleteLocation(newSignature->id());
+			}
+			UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
+		}
+
+		_memory->emptyTrash();
+	}
+
+	if(info)
+	{
+		info->variance = variance;
+		info->inliers = inliersCount;
+		info->matches = correspondences;
+		info->features = nFeatures;
+		info->localMapSize = (int)localMap_.size();
+	}
+
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
+			timer.elapsed(),
+			output.isNull()?"true":"false",
+			nFeatures,
+			inliersCount,
+			correspondences,
+			variance,
+			(int)localMap_.size(),
+			(int)_memory->getVWDictionary()->getVisualWords().size(),
+			(int)_memory->getStMem().size());
+	return output;
+}
+
+} // namespace rtabmap

corelib/src/OdometryICP.cpp

@@ -0,0 +1,216 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+
+namespace rtabmap {
+
+OdometryICP::OdometryICP(int decimation,
+		float voxelSize,
+		int samples,
+		float maxCorrespondenceDistance,
+		int maxIterations,
+		float correspondenceRatio,
+		bool pointToPlane,
+		const ParametersMap & odometryParameter) :
+	Odometry(odometryParameter),
+	_decimation(decimation),
+	_voxelSize(voxelSize),
+	_samples(samples),
+	_maxCorrespondenceDistance(maxCorrespondenceDistance),
+	_maxIterations(maxIterations),
+	_correspondenceRatio(correspondenceRatio),
+	_pointToPlane(pointToPlane),
+	_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
+	_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
+{
+}
+
+void OdometryICP::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+	_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
+	_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
+{
+	UTimer timer;
+	Transform output;
+
+	bool hasConverged = false;
+	double variance = 0;
+	unsigned int minPoints = 100;
+	int correspondences = 0;
+	if(!data.depthOrRightRaw().empty())
+	{
+		if(data.depthOrRightRaw().type() == CV_8UC1)
+		{
+			UERROR("ICP 3D cannot be done on stereo images!");
+			return output;
+		}
+
+		if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
+		{
+			UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
+			return output;
+		}
+		const CameraModel & cameraModel = data.cameraModels()[0];
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
+						data.depthOrRightRaw(),
+						cameraModel.fx(),
+						cameraModel.fy(),
+						cameraModel.cx(),
+						cameraModel.cy(),
+						_decimation,
+						this->getMaxDepth(),
+						_voxelSize,
+						_samples,
+						cameraModel.localTransform());
+
+		if(_pointToPlane)
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
+
+			std::vector<int> indices;
+			newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
+			if(newCloudXYZ->size() != newCloud->size())
+			{
+				UWARN("removed nan normals...");
+			}
+
+			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
+				Transform transform = util3d::icpPointToPlane(
+						newCloud,
+						_previousCloudNormal,
+						_maxCorrespondenceDistance,
+						_maxIterations,
+						hasConverged,
+						*newCloudRegistered);
+
+				util3d::computeVarianceAndCorrespondences(
+						newCloudRegistered,
+						_previousCloudNormal,
+						_maxCorrespondenceDistance,
+						variance,
+						correspondences);
+
+				// verify if there are enough correspondences
+				float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
+
+				if(!transform.isNull() && hasConverged &&
+		   	   	   correspondencesRatio >= _correspondenceRatio)
+				{
+					output = transform;
+					_previousCloudNormal = newCloud;
+				}
+				else
+				{
+					UWARN("Transform not valid (hasConverged=%s variance = %f)",
+							hasConverged?"true":"false", variance);
+				}
+			}
+			else if(newCloud->size() > minPoints)
+			{
+				output.setIdentity();
+				_previousCloudNormal = newCloud;
+			}
+		}
+		else
+		{
+			//point to point
+			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+				Transform transform = util3d::icp(
+						newCloudXYZ,
+						_previousCloud,
+						_maxCorrespondenceDistance,
+						_maxIterations,
+						hasConverged,
+						*newCloudRegistered);
+
+				util3d::computeVarianceAndCorrespondences(
+						newCloudRegistered,
+						_previousCloud,
+						_maxCorrespondenceDistance,
+						variance,
+						correspondences);
+
+				// verify if there are enough correspondences
+				float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
+
+				if(!transform.isNull() && hasConverged &&
+				   correspondencesRatio >= _correspondenceRatio)
+				{
+					output = transform;
+					_previousCloud = newCloudXYZ;
+				}
+				else
+				{
+					UWARN("Transform not valid (hasConverged=%s variance = %f)",
+							hasConverged?"true":"false", variance);
+				}
+			}
+			else if(newCloudXYZ->size() > minPoints)
+			{
+				output.setIdentity();
+				_previousCloud = newCloudXYZ;
+			}
+		}
+	}
+	else
+	{
+		UERROR("Depth is empty?!?");
+	}
+
+	if(info)
+	{
+		info->variance = variance;
+		info->inliers = correspondences;
+	}
+
+	UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
+			timer.elapsed(),
+			hasConverged?"true":"false",
+			variance,
+			(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
+
+	return output;
+}
+
+} // namespace rtabmap

corelib/src/OdometryOpticalFlow.cpp

@@ -0,0 +1,561 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_features.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UMath.h"
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/video/tracking.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+namespace rtabmap {
+
+OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
+	Odometry(parameters),
+	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+	flowIterations_(Parameters::defaultOdomFlowIterations()),
+	flowEps_(Parameters::defaultOdomFlowEps()),
+	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	stereoWinSize_(Parameters::defaultStereoWinSize()),
+	stereoIterations_(Parameters::defaultStereoIterations()),
+	stereoEps_(Parameters::defaultStereoEps()),
+	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
+	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
+	subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
+	subPixIterations_(Parameters::defaultOdomSubPixIterations()),
+	subPixEps_(Parameters::defaultOdomSubPixEps()),
+	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+{
+	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
+	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
+
+	ParametersMap::const_iterator iter;
+	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
+	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+	{
+		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
+	}
+
+	ParametersMap customParameters;
+	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	feature2D_ = Feature2D::create(detectorStrategy, customParameters);
+}
+
+OdometryOpticalFlow::~OdometryOpticalFlow()
+{
+	delete feature2D_;
+}
+
+
+void OdometryOpticalFlow::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+	refFrame_ = cv::Mat();
+	refCorners_.clear();
+	refCorners3D_->clear();
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpticalFlow::computeTransform(
+		const SensorData & data,
+		OdometryInfo * info)
+{
+	UTimer timer;
+	Transform output;
+	if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
+	{
+		UERROR("Calibrated stereo camera required");
+		return output;
+	}
+	if(!data.depthRaw().empty() &&
+		(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
+	{
+		UERROR("Calibrated camera required (multi-cameras not supported).");
+		return output;
+	}
+
+	double variance = 0;
+	int inliers = 0;
+	int correspondences = 0;
+
+	if(info)
+	{
+		info->type = 1;
+	}
+
+	cv::Mat newLeftFrame;
+	// convert to grayscale
+	if(data.imageRaw().channels() > 1)
+	{
+		cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newLeftFrame = data.imageRaw().clone();
+	}
+
+	std::vector<cv::Point2f> newCorners;
+	UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
+			(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1);
+	if(!refFrame_.empty() &&
+	   ((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) &&
+	   refCorners_.size() &&
+	   refCorners3D_->size())
+	{
+		UASSERT_MSG(refCorners_.size() == refCorners3D_->size(),
+				uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
+
+		// make guess
+		bool flowGuessByMotion = true;
+		cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
+		Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
+		Transform guess = (this->previousTransform() * localTransform).inverse();
+		cv::Mat R = (cv::Mat_<double>(3,3) <<
+				(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+				(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+				(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+		cv::Mat rvec(1,3, CV_64FC1);
+		cv::Rodrigues(R, rvec);
+		cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+		std::vector<cv::Point3f> objectPoints(refCorners3D_->size());
+		for(unsigned int i=0; i<objectPoints.size(); ++i)
+		{
+			objectPoints[i].x = refCorners3D_->at(i).x;
+			objectPoints[i].y = refCorners3D_->at(i).y;
+			objectPoints[i].z = refCorners3D_->at(i).z;
+		}
+		if(flowGuessByMotion && !this->previousTransform().isIdentity())
+		{
+			UDEBUG("project points to new image");
+			cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
+		}
+
+		// Find features in the new left image
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2);
+		cv::calcOpticalFlowPyrLK(
+				refFrame_,
+				newLeftFrame,
+				refCorners_,
+				newCorners,
+				status,
+				err,
+				cv::Size(winSize, winSize),
+				(newCorners.size()||!flowGuessByMotion)?flowMaxLevel_:flowMaxLevel_*2,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
+		UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DKept(new pcl::PointCloud<pcl::PointXYZ>);
+		refCorners3DKept->resize(status.size());
+		std::vector<cv::Point3f> objectPointsKept(status.size());
+		std::vector<cv::Point2f> refCornersKept(status.size());
+		std::vector<cv::Point2f> newCornersKept(status.size());
+		int ki = 0;
+		for(unsigned int i=0; i<status.size(); ++i)
+		{
+			if(status[i])
+			{
+				refCorners3DKept->at(ki) = refCorners3D_->at(i);
+				objectPointsKept[ki] = objectPoints[i];
+				refCornersKept[ki] = refCorners_[i];
+				newCornersKept[ki] = newCorners[i];
+				++ki;
+			}
+		}
+		refCorners3DKept->resize(ki);
+		objectPointsKept.resize(ki);
+		refCornersKept.resize(ki);
+		newCornersKept.resize(ki);
+
+		if(ki && ki >= this->getMinInliers())
+		{
+			if(this->getEstimationType() == 1) // PnP
+			{
+				// find correspondences
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners = refCornersKept;
+					info->newCorners = newCornersKept;
+				}
+
+				correspondences = refCornersKept.size();
+
+				if(correspondences >= this->getMinInliers())
+				{
+					//PnPRansac
+					std::vector<int> inliersV;
+					cv::solvePnPRansac(
+							objectPointsKept,
+							newCornersKept,
+							K,
+							cv::Mat(),
+							rvec,
+							tvec,
+							true,
+							this->getIterations(),
+							this->getPnPReprojError(),
+#if CV_MAJOR_VERSION < 3
+							0, // min inliers
+#else
+							0.99, // confidence
+#endif
+							inliersV,
+							this->getPnPFlags());
+
+					cv::Rodrigues(rvec, R);
+					Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+								   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+								   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+					inliers = (int)inliersV.size();
+					if((int)inliersV.size() >= this->getMinInliers())
+					{
+						// make it incremental
+						output = (localTransform * pnp).inverse();
+						variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
+					}
+					else
+					{
+						UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+					}
+
+					if(this->isInfoDataFilled() && info)
+					{
+						info->cornerInliers = inliersV;
+					}
+				}
+				else
+				{
+					UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+				}
+			}
+			else
+			{
+				// Get 3D correspondences
+				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
+				correspondencesRef->resize(newCornersKept.size());
+				correspondencesNew->resize(newCornersKept.size());
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(newCornersKept.size());
+					info->newCorners.resize(newCornersKept.size());
+				}
+				int oi = 0;
+				if(!data.rightRaw().empty())
+				{
+					// stereo
+					pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
+							newCornersKept,
+							newLeftFrame,
+							data.rightRaw(),
+							data.stereoCameraModel().left().fx(),
+							data.stereoCameraModel().baseline(),
+							data.stereoCameraModel().left().cx(),
+							data.stereoCameraModel().left().cy(),
+							Transform::getIdentity(),
+							stereoWinSize_,
+							stereoMaxLevel_,
+							stereoIterations_,
+							stereoEps_,
+							stereoMaxSlope_);
+
+					UASSERT(newCorners3D->size() == refCorners3DKept->size());
+					for(unsigned int i=0; i<newCorners3D->size(); ++i)
+					{
+						if(pcl::isFinite(newCorners3D->at(i)) && (this->getMaxDepth() <= 0.0f || newCorners3D->at(i).z < this->getMaxDepth()))
+						{
+							//Add 3D correspondences!
+							correspondencesRef->at(oi) = refCorners3DKept->at(i);
+							correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), localTransform);
+							if(this->isInfoDataFilled() && info)
+							{
+								info->refCorners[oi] = refCornersKept[i];
+								info->newCorners[oi] = newCornersKept[i];
+							}
+							++oi;
+						}
+					}// end loop
+				}
+				else
+				{
+					//depth
+					for(unsigned int i=0; i<newCornersKept.size(); ++i)
+					{
+						if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depthRaw().cols)) &&
+						   uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depthRaw().rows)))
+						{
+							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depthRaw(), newCornersKept[i].x, newCorners[i].y,
+									data.cameraModels()[0].cx(), data.cameraModels()[0].cy(), data.cameraModels()[0].fx(), data.cameraModels()[0].fy(), true);
+							if(pcl::isFinite(pt) &&
+								(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
+							{
+								//Add 3D correspondences!
+								correspondencesRef->at(oi) = refCorners3DKept->at(i);
+								correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform);
+
+								if(this->isInfoDataFilled() && info)
+								{
+									info->refCorners[oi] = refCornersKept[i];
+									info->newCorners[oi] = newCornersKept[i];
+								}
+								++oi;
+							}
+						}
+					}
+				}
+				correspondencesRef->resize(oi);
+				correspondencesNew->resize(oi);
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(oi);
+					info->newCorners.resize(oi);
+				}
+				correspondences = oi;
+				UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
+
+				if(correspondences >= this->getMinInliers())
+				{
+					std::vector<int> inliersV;
+					UTimer timerRANSAC;
+					Transform t = util3d::transformFromXYZCorrespondences(
+							correspondencesNew,
+							correspondencesRef,
+							this->getInlierDistance(),
+							this->getIterations(),
+							this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+							&inliersV,
+							&variance);
+					UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
+
+					inliers = (int)inliersV.size();
+					if(!t.isNull() && inliers >= this->getMinInliers())
+					{
+						output = t;
+					}
+					else
+					{
+						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+					}
+
+					if(this->isInfoDataFilled() && info)
+					{
+						info->cornerInliers = inliersV;
+					}
+				}
+				else
+				{
+					UWARN("Not enough correspondences (%d)", correspondences);
+				}
+			}
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+	}
+
+	newCorners.clear();
+	if(!output.isNull())
+	{
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			cv::KeyPoint::convert(data.keypoints(), newCorners);
+		}
+		else
+		{
+			// generate kpts
+			std::vector<cv::KeyPoint> newKtps;
+			cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					UDEBUG("cv::cornerSubPix() begin");
+					cv::cornerSubPix(newLeftFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+					UDEBUG("cv::cornerSubPix() end");
+				}
+			}
+		}
+
+		if((int)newCorners.size() >= this->getMinInliers())
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
+			newCorners3D->resize(newCorners.size());
+			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
+			int oi=0;
+			if(!data.rightRaw().empty())
+			{
+				/// stereo
+				pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
+						newCorners,
+						newLeftFrame,
+						data.rightRaw(),
+						data.stereoCameraModel().left().fx(),
+						data.stereoCameraModel().baseline(),
+						data.stereoCameraModel().left().cx(),
+						data.stereoCameraModel().left().cy(),
+						Transform::getIdentity(),
+						stereoWinSize_,
+						stereoMaxLevel_,
+						stereoIterations_,
+						stereoEps_,
+						stereoMaxSlope_);
+				UASSERT(refCorners3DTmp->size() == newCorners.size());
+				for(unsigned int i=0; i<newCorners.size(); ++i)
+				{
+					if(pcl::isFinite(refCorners3DTmp->at(i)) &&
+						(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
+					{
+						newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform());
+						newCornersFiltered[oi] = newCorners[i];
+						++oi;
+					}
+				}
+			}
+			else
+			{
+				// depth
+				for(unsigned int i=0; i<newCorners.size(); ++i)
+				{
+					if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
+					   uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
+					{
+						pcl::PointXYZ pt = util3d::projectDepthTo3D(
+								data.depthRaw(),
+								newCorners[i].x,
+								newCorners[i].y,
+								data.cameraModels()[0].cx(),
+								data.cameraModels()[0].cy(),
+								data.cameraModels()[0].fx(),
+								data.cameraModels()[0].fy(),
+								true);
+						if(pcl::isFinite(pt) &&
+							(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
+						{
+							newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
+							newCornersFiltered[oi] = newCorners[i];
+							++oi;
+						}
+					}
+				}
+			}
+			newCornersFiltered.resize(oi);
+			newCorners3D->resize(oi);
+
+			if((int)newCornersFiltered.size() >= this->getMinInliers())
+			{
+				refFrame_ = newLeftFrame;
+				refCorners_ = newCornersFiltered;
+				refCorners3D_ = newCorners3D;
+			}
+			else
+			{
+				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
+						(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
+				output.setNull();
+			}
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+			output.setNull();
+		}
+	}
+
+	if(info)
+	{
+		info->type = 1;
+		info->variance = variance;
+		info->inliers = inliers;
+		info->features = (int)newCorners.size();
+		info->matches = correspondences;
+	}
+
+	UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			output.isNull()?"true":"false",
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			!output.isNull()?"true":"false");
+
+	return output;
+}
+
+} // namespace rtabmap

corelib/src/OdometryThread.cpp

@@ -0,0 +1,162 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+
+namespace rtabmap {
+
+OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
+	_odometry(odometry),
+	_dataBufferMaxSize(dataBufferMaxSize),
+	_resetOdometry(false)
+{
+	UASSERT(_odometry != 0);
+}
+
+OdometryThread::~OdometryThread()
+{
+	this->unregisterFromEventsManager();
+	this->join(true);
+	if(_odometry)
+	{
+		delete _odometry;
+	}
+	UDEBUG("");
+}
+
+void OdometryThread::handleEvent(UEvent * event)
+{
+	if(this->isRunning())
+	{
+		if(event->getClassName().compare("CameraEvent") == 0)
+		{
+			CameraEvent * cameraEvent = (CameraEvent*)event;
+			if(cameraEvent->getCode() == CameraEvent::kCodeData)
+			{
+				this->addData(cameraEvent->data());
+			}
+			else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
+			{
+				this->post(new CameraEvent()); // forward the event
+			}
+		}
+		else if(event->getClassName().compare("OdometryResetEvent") == 0)
+		{
+			_resetOdometry = true;
+		}
+	}
+}
+
+void OdometryThread::mainLoopKill()
+{
+	_dataAdded.release();
+}
+
+//============================================================
+// MAIN LOOP
+//============================================================
+void OdometryThread::mainLoop()
+{
+	if(_resetOdometry)
+	{
+		_odometry->reset();
+		_resetOdometry = false;
+	}
+
+	SensorData data;
+	if(getData(data))
+	{
+		OdometryInfo info;
+		Transform pose = _odometry->process(data, &info);
+		// a null pose notify that odometry could not be computed
+		double variance = info.variance>0?info.variance:1;
+		this->post(new OdometryEvent(data, pose, variance, variance, info));
+	}
+}
+
+void OdometryThread::addData(const SensorData & data)
+{
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryBOW*>(_odometry) == 0)
+	{
+		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
+		{
+			ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
+			return;
+		}
+	}
+	else
+	{
+		// Mono and BOW can accept RGB only
+		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
+		{
+			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
+			return;
+		}
+	}
+
+	bool notify = true;
+	_dataMutex.lock();
+	{
+		_dataBuffer.push_back(data);
+		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+		{
+			UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+			_dataBuffer.pop_front();
+			notify = false;
+		}
+	}
+	_dataMutex.unlock();
+
+	if(notify)
+	{
+		_dataAdded.release();
+	}
+}
+
+bool OdometryThread::getData(SensorData & data)
+{
+	bool dataFilled = false;
+	_dataAdded.acquire();
+	_dataMutex.lock();
+	{
+		if(!_dataBuffer.empty())
+		{
+			data = _dataBuffer.front();
+			_dataBuffer.pop_front();
+			dataFilled = true;
+		}
+	}
+	_dataMutex.unlock();
+	return dataFilled;
+}
+
+} // namespace rtabmap

corelib/src/Parameters.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <math.h>
 #include <stdlib.h>
+#include <sstream>
 
 namespace rtabmap
 {
@@ -112,7 +113,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atof(iter->second.c_str());
+		value = uStr2Float(iter->second);
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
@@ -120,7 +121,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
-		value = atof(iter->second.c_str());
+		value = uStr2Double(iter->second);
 	}
 }
 void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)

corelib/src/RtabmapThread.cpp

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/UserDataEvent.h"
+#include "rtabmap/core/Memory.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -44,9 +46,13 @@ namespace rtabmap {
 RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
+		_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		_frameRateTimer(new UTimer()),
 		_rtabmap(rtabmap),
-		_paused(false)
+		_paused(false),
+		lastPose_(Transform::getIdentity()),
+		_rotVariance(0),
+		_transVariance(0)
 
 {
 	UASSERT(rtabmap != 0);
@@ -82,8 +88,17 @@ void RtabmapThread::clearBufferedData()
 	_dataMutex.lock();
 	{
 		_dataBuffer.clear();
+		lastPose_.setIdentity();
+		_rotVariance = 0;
+		_transVariance = 0;
 	}
 	_dataMutex.unlock();
+
+	_userDataMutex.lock();
+	{
+		_userData = cv::Mat();
+	}
+	_userDataMutex.unlock();
 }
 
 void RtabmapThread::setDetectorRate(float rate)
@@ -92,52 +107,50 @@ void RtabmapThread::setDetectorRate(float rate)
 	_rate = rate;
 }
 
-void RtabmapThread::setBufferSize(int bufferSize)
+void RtabmapThread::setDataBufferSize(unsigned int size)
 {
-	UASSERT(bufferSize >= 0);
-	_dataBufferMaxSize = bufferSize;
+	_dataBufferMaxSize = size;
 }
 
-void RtabmapThread::publishMap(bool optimized, bool full) const
+void RtabmapThread::createIntermediateNodes(bool enabled)
+{
+	enabled = _createIntermediateNodes;
+}
+
+void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
+	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 
-	_rtabmap->get3DMap(signatures,
-			poses,
-			constraints,
-			mapIds,
-			optimized,
-			full);
+	if(graphOnly)
+	{
+		_rtabmap->getGraph(poses,
+				constraints,
+				optimized,
+				full,
+				&signatures);
+	}
+	else
+	{
+		_rtabmap->get3DMap(
+				signatures,
+				poses,
+				constraints,
+				optimized,
+				full);
+	}
 
-	this->post(new RtabmapEvent3DMap(signatures,
+	this->post(new RtabmapEvent3DMap(
+			signatures,
 			poses,
-			constraints,
-			mapIds));
+			constraints));
 }
 
-void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
-{
-	std::map<int, Signature> signatures;
-	std::map<int, Transform> poses;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-
-	_rtabmap->getGraph(poses,
-			constraints,
-			mapIds,
-			optimized,
-			full);
-
-	this->post(new RtabmapEvent3DMap(signatures,
-			poses,
-			constraints,
-			mapIds));
-}
-
-
 void RtabmapThread::mainLoopKill()
 {
 	this->clearBufferedData();
@@ -163,6 +176,8 @@ void RtabmapThread::mainLoop()
 	}
 	_stateMutex.unlock();
 
+	int id = 0;
+	cv::Mat userData;
 	switch(state)
 	{
 	case kStateDetecting:
@@ -170,11 +185,17 @@ void RtabmapThread::mainLoop()
 		break;
 	case kStateInit:
 		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
+		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
+		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
+		UASSERT(_dataBufferMaxSize >= 0);
+		UASSERT(_rate >= 0.0f);
 		_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
 		break;
 	case kStateChangingParameters:
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
 		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
@@ -197,36 +218,61 @@ void RtabmapThread::mainLoop()
 	case kStateDumpingPrediction:
 		_rtabmap->dumpPrediction();
 		break;
-	case kStateGeneratingDOTGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"));
+	case kStateExportingDOTGraph:
+		_rtabmap->generateDOTGraph(
+				parameters.at("path"),
+				atoi(parameters.at("id").c_str()),
+				atoi(parameters.at("margin").c_str()));
 		break;
-	case kStateGeneratingDOTLocalGraph:
-		_rtabmap->generateDOTGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
-		break;
-	case kStateGeneratingTOROGraphLocal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStateGeneratingTOROGraphGlobal:
-		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStateExportingPoses:
+		_rtabmap->exportPoses(
+				parameters.at("path"),
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				atoi(parameters.at("type").c_str()));
 		break;
 	case kStateCleanDataBuffer:
 		this->clearBufferedData();
 		break;
-	case kStatePublishingMapLocal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingMapGlobal:
-		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
-		break;
-	case kStatePublishingTOROGraphLocal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
-		break;
-	case kStatePublishingTOROGraphGlobal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
+	case kStatePublishingMap:
+		this->publishMap(
+				uStr2Bool(parameters.at("optimized")),
+				uStr2Bool(parameters.at("global")),
+				uStr2Bool(parameters.at("graph_only")));
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
 		break;
+	case kStateAddingUserData:
+		_userDataMutex.lock();
+		{
+			userData = _userData;
+			_userData = cv::Mat();
+		}
+		_userDataMutex.unlock();
+		_rtabmap->setUserData(0, userData);
+		break;
+	case kStateSettingGoal:
+		id = atoi(parameters.at("id").c_str());
+		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
+		{
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
+		}
+		if(id <= 0 || !_rtabmap->computePath(id, true))
+		{
+			UERROR("Failed to set a goal to location=%d.", id);
+		}
+		this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath()));
+		break;
+	case kStateCancellingGoal:
+		_rtabmap->clearPath(0);
+		break;
+	case kStateLabelling:
+		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
+		{
+			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()));
+		}
+		break;
 	default:
 		UFATAL("Invalid state !?!?");
 		break;
@@ -240,18 +286,48 @@ void RtabmapThread::handleEvent(UEvent* event)
 	{
 		UDEBUG("CameraEvent");
 		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeImageDepth)
+		if(e->getCode() == CameraEvent::kCodeData)
 		{
-			this->addData(e->data());
+			this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
 		}
 	}
 	else if(event->getClassName().compare("OdometryEvent") == 0)
 	{
 		UDEBUG("OdometryEvent");
 		OdometryEvent * e = (OdometryEvent*)event;
-		if(e->isValid())
+		if(!e->pose().isNull())
 		{
-			this->addData(e->data());
+			this->addData(*e);
+		}
+		else
+		{
+			lastPose_.setNull();
+		}
+	}
+	else if(event->getClassName().compare("UserDataEvent") == 0)
+	{
+		if(!_paused)
+		{
+			UDEBUG("UserDataEvent");
+			bool updated = false;
+			UserDataEvent * e = (UserDataEvent*)event;
+			_userDataMutex.lock();
+			if(!e->data().empty())
+			{
+				updated = !_userData.empty();
+				_userData = e->data();
+			}
+			_userDataMutex.unlock();
+			if(updated)
+			{
+				UWARN("New user data received before the last one was processed... replacing "
+					"user data with this new one. Note that UserDataEvent should be used only "
+					"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
+			}
+			else
+			{
+				pushNewState(kStateAddingUserData);
+			}
 		}
 	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
@@ -262,8 +338,8 @@ void RtabmapThread::handleEvent(UEvent* event)
 		{
 			ULOGGER_DEBUG("CMD_INIT");
 			ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-			UASSERT(!rtabmapEvent->getStr().empty());
-			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->getStr())).second);
+			UASSERT(rtabmapEvent->value1().isStr());
+			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
 			pushNewState(kStateInit, parameters);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdClose)
@@ -288,46 +364,30 @@ void RtabmapThread::handleEvent(UEvent* event)
 		}
 		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
 		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
 			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isStr());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
+			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			pushNewState(kStateGeneratingDOTGraph, param);
+			param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
+			param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingDOTGraph, param);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTLocalGraph)
+		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
 		{
-			std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
-			UASSERT(values.size() == 3);
-
-			ULOGGER_DEBUG("CMD_GENERATE_DOT_LOCAL_GRAPH");
+			ULOGGER_DEBUG("CMD_EXPORT_POSES");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isStr());
+			UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("path", *values.begin()));
-			param.insert(ParametersPair("id", *(++values.begin())));
-			param.insert(ParametersPair("margin", *values.rbegin()));
-			pushNewState(kStateGeneratingDOTLocalGraph, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphLocal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphLocal, param);
-
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateTOROGraphGlobal)
-		{
-			UASSERT(!rtabmapEvent->getStr().empty());
-
-			ULOGGER_DEBUG("CMD_GENERATE_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStateGeneratingTOROGraphGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
+			param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
+			pushNewState(kStateExportingPoses, param);
 
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
@@ -335,33 +395,17 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
 			pushNewState(kStateCleanDataBuffer);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapLocal)
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
 		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_LOCAL");
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			UASSERT(rtabmapEvent->value1().isBool());
+			UASSERT(rtabmapEvent->value2().isBool());
+			UASSERT(rtabmapEvent->value3().isBool());
 			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMapGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingMapGlobal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphLocal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_LOCAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphLocal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPublishTOROGraphGlobal)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_TORO_GRAPH_GLOBAL");
-			ParametersMap param;
-			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
-			pushNewState(kStatePublishingTOROGraphGlobal, param);
+			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
+			param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
+			pushNewState(kStatePublishingMap, param);
 		}
 		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
 		{
@@ -373,6 +417,30 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_PAUSE");
 			_paused = !_paused;
 		}
+		else if(cmd == RtabmapEventCmd::kCmdGoal)
+		{
+			ULOGGER_DEBUG("CMD_GOAL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
+			ParametersMap param;
+			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
+			pushNewState(kStateSettingGoal, param);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
+		{
+			ULOGGER_DEBUG("CMD_CANCEL_GOAL");
+			pushNewState(kStateCancellingGoal);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdLabel)
+		{
+			ULOGGER_DEBUG("CMD_LABEL");
+			UASSERT(rtabmapEvent->value1().isStr());
+			UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
+			ParametersMap param;
+			param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
+			param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
+			pushNewState(kStateLabelling, param);
+		}
 		else
 		{
 			UWARN("Cmd %d unknown!", cmd);
@@ -390,18 +458,24 @@ void RtabmapThread::handleEvent(UEvent* event)
 //============================================================
 void RtabmapThread::process()
 {
-	SensorData data;
-	getData(data);
-	if(data.isValid())
+	OdometryEvent data;
+	if(_state.empty() && getData(data))
 	{
 		if(_rtabmap->getMemory())
 		{
-			if(_rtabmap->process(data))
+			bool wasPlanning = _rtabmap->getPath().size()>0;
+			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
 				ULOGGER_DEBUG("posting statistics_ event...");
 				this->post(new RtabmapEvent(stats));
+
+				if(wasPlanning && _rtabmap->getPath().size() == 0)
+				{
+					// Goal reached or failed
+					this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus()));
+				}
 			}
 		}
 		else
@@ -411,30 +485,78 @@ void RtabmapThread::process()
 	}
 }
 
-void RtabmapThread::addData(const SensorData & sensorData)
+void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
-		if(!sensorData.isValid())
-		{
-			ULOGGER_ERROR("data not valid !?");
-			return;
-		}
-
+		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
 			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
 			{
-				return;
+				ignoreFrame = true;
 			}
 		}
-		_frameRateTimer->start();
+		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity())
+		{
+			UWARN("Odometry is reset (identity pose detected). Increment map id!");
+			pushNewState(kStateTriggeringMap);
+			_rotVariance = 0;
+			_transVariance = 0;
+		}
+
+		if(ignoreFrame && !_createIntermediateNodes)
+		{
+			return;
+		}
+		else if(!ignoreFrame)
+		{
+			_frameRateTimer->start();
+		}
+
+		lastPose_ = odomEvent.pose();
+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		// FIXME: should merge the transformations/variances like Link::merge();
+		if(maxRotVar > _rotVariance)
+		{
+			_rotVariance = maxRotVar;
+		}
+		if(maxTransVar > _transVariance)
+		{
+			_transVariance = maxTransVar;
+		}
 
 		bool notify = true;
 		_dataMutex.lock();
 		{
-			_dataBuffer.push_back(sensorData);
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
+			if(_rotVariance <= 0)
+			{
+				_rotVariance = 1.0;
+			}
+			if(_transVariance <= 0)
+			{
+				_transVariance = 1.0;
+			}
+			if(ignoreFrame)
+			{
+				// remove data from the frame, keeping only constraints
+				SensorData tmp(
+						cv::Mat(),
+						odomEvent.data().id(),
+						odomEvent.data().stamp(),
+						odomEvent.data().userDataRaw());
+				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
+			}
+			else
+			{
+				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
+			}
+			UDEBUG("Added data %d", odomEvent.data().id());
+
+			_rotVariance = 0;
+			_transVariance = 0;
+			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
 			{
 				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
 				_dataBuffer.pop_front();
@@ -450,7 +572,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 	}
 }
 
-void RtabmapThread::getData(SensorData & image)
+bool RtabmapThread::getData(OdometryEvent & data)
 {
 	ULOGGER_DEBUG("");
 
@@ -458,28 +580,18 @@ void RtabmapThread::getData(SensorData & image)
 	_dataAdded.acquire();
 	ULOGGER_INFO("wake-up");
 
+	bool dataFilled = false;
 	_dataMutex.lock();
 	{
 		if(!_dataBuffer.empty())
 		{
-			image = _dataBuffer.front();
+			data = _dataBuffer.front();
 			_dataBuffer.pop_front();
+			dataFilled = true;
 		}
 	}
 	_dataMutex.unlock();
-}
-
-void RtabmapThread::setDataBufferSize(int size)
-{
-	if(size < 0)
-	{
-		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
-		_dataBufferMaxSize = 0;
-	}
-	else
-	{
-		_dataBufferMaxSize = size;
-	}
+	return dataFilled;
 }
 
 } /* namespace rtabmap */

corelib/src/SensorData.cpp

@@ -27,102 +27,554 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/ULogger.h"
+#include <rtabmap/utilite/UMath.h>
 
 namespace rtabmap
 {
 
-/**
- * An id is automatically generated if id=0.
- */
+// empty constructor
 SensorData::SensorData() :
-	_image(cv::Mat()),
-	_id(0),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity())
+		_id(0),
+		_stamp(0.0),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
 }
 
-SensorData::SensorData(const cv::Mat & image,
-	  int id) :
-	_image(image),
-	_id(id),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity())
+// Appearance-only constructor
+SensorData::SensorData(
+		const cv::Mat & image,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f)
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
+	if(image.rows == 1)
+	{
+		UASSERT(image.type() == CV_8UC1); // Bytes
+		_imageCompressed = image;
+	}
+	else if(!image.empty())
+	{
+		UASSERT(image.type() == CV_8UC1 || // Mono
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
 }
 
-	// Metric constructor
-SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id) :
-	_image(image),
-	_id(id),
-	_depthOrRightImage(depthOrRightImage),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform)
+// Mono constructor
+SensorData::SensorData(
+		const cv::Mat & image,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
-	UASSERT(!_localTransform.isNull());
+	if(image.rows == 1)
+	{
+		UASSERT(image.type() == CV_8UC1); // Bytes
+		_imageCompressed = image;
+	}
+	else if(!image.empty())
+	{
+		UASSERT(image.type() == CV_8UC1 || // Mono
+				image.type() == CV_8UC3);  // RGB
+		_imageRaw = image;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
 }
 
-	// Metric constructor + 2d depth
-SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  const cv::Mat & depth2d,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & pose,
-		  const Transform & localTransform,
-		  int id) :
-	_image(image),
-	_id(id),
-	_depthOrRightImage(depthOrRightImage),
-	_depth2d(depth2d),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform)
+// RGB-D constructor
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
-	UASSERT(!_localTransform.isNull());
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
 }
 
-bool SensorData::empty() const
+// RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		float laserScanMaxRange,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
-	return _image.empty();
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+// Multi-cameras RGB-D constructor
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
+		_cameraModels(cameraModels)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+// Multi-cameras RGB-D constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		float laserScanMaxRange,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
+		_cameraModels(cameraModels)
+{
+	if(rgb.rows == 1)
+	{
+		UASSERT(rgb.type() == CV_8UC1); // Bytes
+		_imageCompressed = rgb;
+	}
+	else if(!rgb.empty())
+	{
+		UASSERT(rgb.type() == CV_8UC1 || // Mono
+				rgb.type() == CV_8UC3);  // RGB
+		_imageRaw = rgb;
+	}
+	if(depth.rows == 1)
+	{
+		UASSERT(depth.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = depth;
+	}
+	else if(!depth.empty())
+	{
+		UASSERT(depth.type() == CV_32FC1 || // Depth in meter
+				depth.type() == CV_16UC1); // Depth in millimetre
+		_depthOrRightRaw = depth;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+// Stereo constructor
+SensorData::SensorData(
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData):
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0),
+		_laserScanMaxRange(0.0f),
+		_stereoCameraModel(cameraModel)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+
+}
+
+// Stereo constructor + 2d laser scan
+SensorData::SensorData(
+		const cv::Mat & laserScan,
+		int laserScanMaxPts,
+		float laserScanMaxRange,
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const StereoCameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(laserScanMaxPts),
+		_laserScanMaxRange(laserScanMaxRange),
+		_stereoCameraModel(cameraModel)
+{
+	if(left.rows == 1)
+	{
+		UASSERT(left.type() == CV_8UC1); // Bytes
+		_imageCompressed = left;
+	}
+	else if(!left.empty())
+	{
+		UASSERT(left.type() == CV_8UC1 || // Mono
+				left.type() == CV_8UC3);  // RGB
+		_imageRaw = left;
+	}
+	if(right.rows == 1)
+	{
+		UASSERT(right.type() == CV_8UC1); // Bytes
+		_depthOrRightCompressed = right;
+	}
+	else if(!right.empty())
+	{
+		UASSERT(right.type() == CV_8UC1); // Mono
+		_depthOrRightRaw = right;
+	}
+
+	if(laserScan.type() == CV_32FC2)
+	{
+		_laserScanRaw = laserScan;
+	}
+	else if(!laserScan.empty())
+	{
+		UASSERT(laserScan.type() == CV_8UC1); // Bytes
+		_laserScanCompressed = laserScan;
+	}
+
+	if(userData.type() == CV_8UC1) // Bytes
+	{
+		_userDataCompressed = userData; // assume compressed
+	}
+	else
+	{
+		_userDataRaw = userData;
+	}
+}
+
+void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
+{
+	if(!_userDataRaw.empty())
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = userDataRaw;
+}
+
+void SensorData::setUserData(const cv::Mat & userData)
+{
+	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
+	{
+		UWARN("Writing new user data over existing user data. This may result in data loss.");
+	}
+	_userDataRaw = cv::Mat();
+	_userDataCompressed = cv::Mat();
+
+	if(!userData.empty())
+	{
+		if(userData.type() == CV_8UC1) // Bytes
+		{
+			_userDataCompressed = userData; // assume compressed
+		}
+		else
+		{
+			_userDataRaw = userData;
+			_userDataCompressed = compressData2(userData);
+		}
+	}
+}
+
+void SensorData::uncompressData()
+{
+	uncompressData(_imageCompressed.empty()?0:&_imageRaw,
+				_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
+				_laserScanCompressed.empty()?0:&_laserScanRaw,
+				_userDataCompressed.empty()?0:&_userDataRaw);
+}
+
+void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
+{
+	uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
+	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
+	{
+		_imageRaw = *imageRaw;
+	}
+	if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
+	{
+		_depthOrRightRaw = *depthRaw;
+	}
+	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
+	{
+		_laserScanRaw = *laserScanRaw;
+	}
+	if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
+	{
+		_userDataRaw = *userDataRaw;
+	}
+}
+
+void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
+{
+	if(imageRaw)
+	{
+		*imageRaw = _imageRaw;
+	}
+	if(depthRaw)
+	{
+		*depthRaw = _depthOrRightRaw;
+	}
+	if(laserScanRaw)
+	{
+		*laserScanRaw = _laserScanRaw;
+	}
+	if(userDataRaw)
+	{
+		*userDataRaw = _userDataRaw;
+	}
+	if( (imageRaw && imageRaw->empty()) ||
+		(depthRaw && depthRaw->empty()) ||
+		(laserScanRaw && laserScanRaw->empty()) ||
+		(userDataRaw && userDataRaw->empty()))
+	{
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
+		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
+		if(imageRaw && imageRaw->empty())
+		{
+			ctImage.start();
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			ctDepth.start();
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			ctLaserScan.start();
+		}
+		if(userDataRaw && userDataRaw->empty())
+		{
+			ctUserData.start();
+		}
+		ctImage.join();
+		ctDepth.join();
+		ctLaserScan.join();
+		ctUserData.join();
+		if(imageRaw && imageRaw->empty())
+		{
+			*imageRaw = ctImage.getUncompressedData();
+			if(imageRaw->empty())
+			{
+				UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
+			}
+		}
+		if(depthRaw && depthRaw->empty())
+		{
+			*depthRaw = ctDepth.getUncompressedData();
+			if(depthRaw->empty())
+			{
+				UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
+			}
+		}
+		if(laserScanRaw && laserScanRaw->empty())
+		{
+			*laserScanRaw = ctLaserScan.getUncompressedData();
+
+			if(laserScanRaw->empty())
+			{
+				UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
+			}
+		}
+		if(userDataRaw && userDataRaw->empty())
+		{
+			*userDataRaw = ctUserData.getUncompressedData();
+
+			if(userDataRaw->empty())
+			{
+				UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
+			}
+		}
+	}
 }
 
 } // namespace rtabmap

corelib/src/Signature.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/Memory.h"
-#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 #include <opencv2/highgui/highgui.hpp>
 
 #include <rtabmap/utilite/UtiLite.h>
@@ -39,51 +39,40 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_weight(-1),
+	_stamp(0.0),
+	_weight(0),
 	_saved(false),
 	_modified(true),
-	_neighborsModified(true),
-	_enabled(false),
-	_fx(0.0f),
-	_fy(0.0f),
-	_cx(0.0f),
-	_cy(0.0f)
+	_linksModified(true),
+	_enabled(false)
 {
 }
 
 Signature::Signature(
 		int id,
 		int mapId,
-		const std::multimap<int, cv::KeyPoint> & words,
-		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
+		int weight,
+		double stamp,
+		const std::string & label,
 		const Transform & pose,
-		const cv::Mat & depth2DCompressed, // in base_link frame
-		const cv::Mat & imageCompressed, // in camera_link frame
-		const cv::Mat & depthCompressed, // in camera_link frame
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform) :
+		const SensorData & sensorData):
 	_id(id),
 	_mapId(mapId),
-	_weight(0),
+	_stamp(stamp),
+	_weight(weight),
+	_label(label),
 	_saved(false),
 	_modified(true),
-	_neighborsModified(true),
-	_words(words),
+	_linksModified(true),
 	_enabled(false),
-	_imageCompressed(imageCompressed),
-	_depthCompressed(depthCompressed),
-	_depth2DCompressed(depth2DCompressed),
-	_fx(fx),
-	_fy(fy),
-	_cx(cx),
-	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform),
-	_words3(words3)
+	_sensorData(sensorData)
 {
+	if(_sensorData.id() == 0)
+	{
+		_sensorData.setId(id);
+	}
+	UASSERT(_sensorData.id() == _id);
 }
 
 Signature::~Signature()
@@ -91,80 +80,79 @@ Signature::~Signature()
 	//UDEBUG("id=%d", _id);
 }
 
-void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
+void Signature::addLinks(const std::list<Link> & links)
 {
-	for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
 	{
-		this->addNeighbor(i->first, i->second);
+		addLink(*iter);
+	}
+}
+void Signature::addLinks(const std::map<int, Link> & links)
+{
+	for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+	{
+		addLink(iter->second);
+	}
+}
+void Signature::addLink(const Link & link)
+{
+	UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
+	UASSERT(link.from() == this->id());
+	std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
+	UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
+	_linksModified = true;
+}
+
+bool Signature::hasLink(int idTo) const
+{
+	return _links.find(idTo) != _links.end();
+}
+
+void Signature::changeLinkIds(int idFrom, int idTo)
+{
+	std::map<int, Link>::iterator iter = _links.find(idFrom);
+	if(iter != _links.end())
+	{
+		Link link = iter->second;
+		_links.erase(iter);
+		link.setTo(idTo);
+		_links.insert(std::make_pair(idTo, link));
+		_linksModified = true;
+		UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
 	}
 }
 
-void Signature::addNeighbor(int neighbor, const Transform & transform)
+void Signature::removeLinks()
 {
-	UDEBUG("Add neighbor %d to %d", neighbor, this->id());
-	_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
-	_neighborsModified = true;
+	if(_links.size())
+		_linksModified = true;
+	_links.clear();
 }
 
-void Signature::removeNeighbor(int neighborId)
+void Signature::removeLink(int idTo)
 {
-	int count = (int)_neighbors.erase(neighborId);
+	int count = (int)_links.erase(idTo);
 	if(count)
 	{
-		_neighborsModified = true;
+		_linksModified = true;
 	}
 }
 
-void Signature::removeNeighbors()
+void Signature::removeVirtualLinks()
 {
-	if(_neighbors.size())
-		_neighborsModified = true;
-	_neighbors.clear();
-}
-
-void Signature::changeNeighborIds(int idFrom, int idTo)
-{
-	std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
-	if(iter != _neighbors.end())
+	for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
 	{
-		Transform t = iter->second;
-		_neighbors.erase(iter);
-		_neighbors.insert(std::pair<int, Transform>(idTo, t));
-		_neighborsModified = true;
-	}
-	UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
-}
-
-void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
-{
-	if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
-	{
-		_neighborsModified=true;
+		if(iter->second.type() == Link::kVirtualClosure)
+		{
+			_links.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
 	}
 }
 
-void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
-{
-	if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
-	{
-		_neighborsModified=true;
-	}
-}
-
-void Signature::changeLoopClosureId(int idFrom, int idTo)
-{
-	std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
-	if(iter != _loopClosureIds.end())
-	{
-		Transform t = iter->second;
-		_loopClosureIds.erase(iter);
-		_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
-		_neighborsModified = true;
-	}
-	UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
-}
-
-
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
@@ -217,102 +205,25 @@ void Signature::removeWord(int wordId)
 	_words3.erase(wordId);
 }
 
-void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
+cv::Mat Signature::getPoseCovariance() const
 {
-	UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
-	_depthCompressed = bytes;
-	_fx=fx;
-	_fy=fy;
-	_cx=cx;
-	_cy=cy;
-}
-
-SensorData Signature::toSensorData()
-{
-	this->uncompressData();
-	return SensorData(_imageRaw,
-			_depthRaw,
-			_depth2DRaw,
-			_fx,
-			_fy,
-			_cx,
-			_cy,
-			_pose,
-			_localTransform,
-			_id);
-}
-
-void Signature::uncompressData()
-{
-	uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
-}
-
-void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw)
-{
-	uncompressDataConst(imageRaw, depthRaw, depth2DRaw);
-	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
+	if(_links.size())
 	{
-		_imageRaw = *imageRaw;
-	}
-	if(depthRaw && !depthRaw->empty() && _depthRaw.empty())
-	{
-		_depthRaw = *depthRaw;
-	}
-	if(depth2DRaw && !depth2DRaw->empty() && _depth2DRaw.empty())
-	{
-		_depth2DRaw = *depth2DRaw;
-	}
-}
-
-void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const
-{
-	if(imageRaw)
-	{
-		*imageRaw = _imageRaw;
-	}
-	if(depthRaw)
-	{
-		*depthRaw = _depthRaw;
-	}
-	if(depth2DRaw)
-	{
-		*depth2DRaw = _depth2DRaw;
-	}
-	if( (imageRaw && imageRaw->empty()) ||
-		(depthRaw && depthRaw->empty()) ||
-		(depth2DRaw && depth2DRaw->empty()))
-	{
-		util3d::CompressionThread ctImage(_imageCompressed, true);
-		util3d::CompressionThread ctDepth(_depthCompressed, true);
-		util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
-		if(imageRaw && imageRaw->empty())
+		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
 		{
-			ctImage.start();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			ctDepth.start();
-		}
-		if(depth2DRaw && depth2DRaw->empty())
-		{
-			ctDepth2D.start();
-		}
-		ctImage.join();
-		ctDepth.join();
-		ctDepth2D.join();
-		if(imageRaw && imageRaw->empty())
-		{
-			*imageRaw = ctImage.getUncompressedData();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			*depthRaw = ctDepth.getUncompressedData();
-		}
-		if(depth2DRaw && depth2DRaw->empty())
-		{
-			*depth2DRaw = ctDepth2D.getUncompressedData();
+			if(iter->second.kNeighbor)
+			{
+				//Assume the first neighbor to be the backward neighbor link
+				if(iter->second.to() < iter->second.from())
+				{
+					covariance = iter->second.infMatrix().inv();
+					break;
+				}
+			}
 		}
 	}
+	return covariance;
 }
 
 } //namespace rtabmap

corelib/src/SimpleIni.h

@@ -1,4 +1,4 @@
-/** @mainpage
+/**
 
     <table>
         <tr><th>Library     <td>SimpleIni

corelib/src/Statistics.cpp

@@ -42,7 +42,8 @@ Statistics::Statistics() :
 	_extended(0),
 	_refImageId(0),
 	_loopClosureId(0),
-	_localLoopClosureId(0)
+	_localLoopClosureId(0),
+	_currentGoalId(0)
 {
 	_defaultDataInitialized = true;
 }

corelib/src/Transform.cpp

@@ -31,94 +31,89 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
 #include <iomanip>
 
 namespace rtabmap {
 
-Transform::Transform() : data_(12)
+Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
 {
-	data_[0] = 0.0f;
-	data_[1] = 0.0f;
-	data_[2] = 0.0f;
-	data_[3] = 0.0f;
-	data_[4] = 0.0f;
-	data_[5] = 0.0f;
-	data_[6] = 0.0f;
-	data_[7] = 0.0f;
-	data_[8] = 0.0f;
-	data_[9] = 0.0f;
-	data_[10] = 0.0f;
-	data_[11] = 0.0f;
 }
 
 // rotation matrix r## and origin o##
-Transform::Transform(float r11, float r12, float r13, float o14,
-				     float r21, float r22, float r23, float o24,
-				     float r31, float r32, float r33, float o34) :
-	data_(12)
+Transform::Transform(
+		float r11, float r12, float r13, float o14,
+		float r21, float r22, float r23, float o24,
+		float r31, float r32, float r33, float o34)
 {
-	data_[0] = r11;
-	data_[1] = r12;
-	data_[2] = r13;
-	data_[3] = o14;
-	data_[4] = r21;
-	data_[5] = r22;
-	data_[6] = r23;
-	data_[7] = o24;
-	data_[8] = r31;
-	data_[9] = r32;
-	data_[10] = r33;
-	data_[11] = o34;
+	data_ = (cv::Mat_<float>(3,4) <<
+			r11, r12, r13, o14,
+			r21, r22, r23, o24,
+			r31, r32, r33, o34);
+}
+
+Transform::Transform(const cv::Mat & transformationMatrix)
+{
+	UASSERT(transformationMatrix.cols == 4 &&
+			transformationMatrix.rows == 3 &&
+			transformationMatrix.type() == CV_32FC1);
+	data_ = transformationMatrix;
 }
 
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
 {
 	Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
-	*this = util3d::transformFromEigen3f(t);
+	*this = fromEigen3f(t);
+}
+
+Transform::Transform(float x, float y, float theta)
+{
+	Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
+	*this = fromEigen3f(t);
 }
 
 bool Transform::isNull() const
 {
-	return (data_[0] == 0.0f &&
-			data_[1] == 0.0f &&
-			data_[2] == 0.0f &&
-			data_[3] == 0.0f &&
-			data_[4] == 0.0f &&
-			data_[5] == 0.0f &&
-			data_[6] == 0.0f &&
-			data_[7] == 0.0f &&
-			data_[8] == 0.0f &&
-			data_[9] == 0.0f &&
-			data_[10] == 0.0f &&
-			data_[11] == 0.0f) ||
-			uIsNan(data_[0]) ||
-			uIsNan(data_[1]) ||
-			uIsNan(data_[2]) ||
-			uIsNan(data_[3]) ||
-			uIsNan(data_[4]) ||
-			uIsNan(data_[5]) ||
-			uIsNan(data_[6]) ||
-			uIsNan(data_[7]) ||
-			uIsNan(data_[8]) ||
-			uIsNan(data_[9]) ||
-			uIsNan(data_[10]) ||
-			uIsNan(data_[11]);
+	return (data()[0] == 0.0f &&
+			data()[1] == 0.0f &&
+			data()[2] == 0.0f &&
+			data()[3] == 0.0f &&
+			data()[4] == 0.0f &&
+			data()[5] == 0.0f &&
+			data()[6] == 0.0f &&
+			data()[7] == 0.0f &&
+			data()[8] == 0.0f &&
+			data()[9] == 0.0f &&
+			data()[10] == 0.0f &&
+			data()[11] == 0.0f) ||
+			uIsNan(data()[0]) ||
+			uIsNan(data()[1]) ||
+			uIsNan(data()[2]) ||
+			uIsNan(data()[3]) ||
+			uIsNan(data()[4]) ||
+			uIsNan(data()[5]) ||
+			uIsNan(data()[6]) ||
+			uIsNan(data()[7]) ||
+			uIsNan(data()[8]) ||
+			uIsNan(data()[9]) ||
+			uIsNan(data()[10]) ||
+			uIsNan(data()[11]);
 }
 
 bool Transform::isIdentity() const
 {
-	return data_[0] == 1.0f &&
-			data_[1] == 0.0f &&
-			data_[2] == 0.0f &&
-			data_[3] == 0.0f &&
-			data_[4] == 0.0f &&
-			data_[5] == 1.0f &&
-			data_[6] == 0.0f &&
-			data_[7] == 0.0f &&
-			data_[8] == 0.0f &&
-			data_[9] == 0.0f &&
-			data_[10] == 1.0f &&
-			data_[11] == 0.0f;
+	return data()[0] == 1.0f &&
+			data()[1] == 0.0f &&
+			data()[2] == 0.0f &&
+			data()[3] == 0.0f &&
+			data()[4] == 0.0f &&
+			data()[5] == 1.0f &&
+			data()[6] == 0.0f &&
+			data()[7] == 0.0f &&
+			data()[8] == 0.0f &&
+			data()[9] == 0.0f &&
+			data()[10] == 1.0f &&
+			data()[11] == 0.0f;
 }
 
 void Transform::setNull()
@@ -131,36 +126,42 @@ void Transform::setIdentity()
 	*this = getIdentity();
 }
 
-Transform Transform::getIdentity()
+float Transform::theta() const
 {
-	return Transform(1,0,0,0,
-					 0,1,0,0,
-					 0,0,1,0);
+	float roll, pitch, yaw;
+	this->getEulerAngles(roll, pitch, yaw);
+	return yaw;
 }
 
 Transform Transform::inverse() const
 {
-	Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
-	return util3d::transformFromEigen4f(m.inverse());
+	return fromEigen4f(toEigen4f().inverse());
 }
 
 Transform Transform::rotation() const
 {
-	return Transform(data_[0], data_[1], data_[2], 0,
-					 data_[4], data_[5], data_[6], 0,
-					 data_[8], data_[9], data_[10], 0);
+	return Transform(
+			data()[0], data()[1], data()[2], 0,
+			data()[4], data()[5], data()[6], 0,
+			data()[8], data()[9], data()[10], 0);
 }
 
 Transform Transform::translation() const
 {
-	return Transform(1,0,0, data_[3],
-					 0,1,0, data_[7],
-					 0,0,1, data_[11]);
+	return Transform(1,0,0, data()[3],
+					 0,1,0, data()[7],
+					 0,0,1, data()[11]);
 }
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
 {
-	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
+	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
+}
+
+void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
+{
+	float x,y,z;
+	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
 }
 
 void Transform::getTranslation(float & x, float & y, float & z) const
@@ -172,12 +173,22 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 
 float Transform::getNorm() const
 {
-	return std::sqrt(this->getNormSquared());
+	return uNorm(this->x(), this->y(), this->z());
 }
 
 float Transform::getNormSquared() const
 {
-	return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
+	return uNormSquared(this->x(), this->y(), this->z());
+}
+
+float Transform::getDistance(const Transform & t) const
+{
+	return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
+}
+
+float Transform::getDistanceSquared(const Transform & t) const
+{
+	return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
 }
 
 std::string Transform::prettyPrint() const
@@ -189,9 +200,7 @@ std::string Transform::prettyPrint() const
 
 Transform Transform::operator*(const Transform & t) const
 {
-	Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
-	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
-	return util3d::transformFromEigen4f(m1*m2);
+	return fromEigen4f(toEigen4f()*t.toEigen4f());
 }
 
 Transform & Transform::operator*=(const Transform & t)
@@ -202,7 +211,7 @@ Transform & Transform::operator*=(const Transform & t)
 
 bool Transform::operator==(const Transform & t) const
 {
-	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+	return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
 }
 
 bool Transform::operator!=(const Transform & t) const
@@ -223,5 +232,87 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 	return os;
 }
 
+Eigen::Matrix4f Transform::toEigen4f() const
+{
+	Eigen::Matrix4f m;
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 data()[8], data()[9], data()[10], data()[11],
+		 0,0,0,1;
+	return m;
+}
+Eigen::Matrix4d Transform::toEigen4d() const
+{
+	Eigen::Matrix4d m;
+	m << data()[0], data()[1], data()[2], data()[3],
+		 data()[4], data()[5], data()[6], data()[7],
+		 data()[8], data()[9], data()[10], data()[11],
+		 0,0,0,1;
+	return m;
+}
+
+Eigen::Affine3f Transform::toEigen3f() const
+{
+	return Eigen::Affine3f(toEigen4f());
+}
+
+Eigen::Affine3d Transform::toEigen3d() const
+{
+	return Eigen::Affine3d(toEigen4d());
+}
+
+Eigen::Quaternionf Transform::getQuaternionf() const
+{
+	return Eigen::Quaternionf(this->toEigen3f().rotation()).normalized();
+}
+
+Eigen::Quaterniond Transform::getQuaterniond() const
+{
+	return Eigen::Quaterniond(this->toEigen3d().rotation()).normalized();
+}
+
+Transform Transform::getIdentity()
+{
+	return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
+}
+
+Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+Transform Transform::fromEigen3f(const Eigen::Affine3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+Transform Transform::fromEigen3f(const Eigen::Isometry3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
 
 }

corelib/src/VWDictionary.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/VWDictionary.h"
-#include "VisualWord.h"
+#include "rtabmap/core/VisualWord.h"
 
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/DBDriver.h"
@@ -34,7 +34,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/utilite/UtiLite.h"
 
+#include <opencv2/opencv_modules.hpp>
+
+#if CV_MAJOR_VERSION < 3
 #include <opencv2/gpu/gpu.hpp>
+#else
+#include <opencv2/core/cuda.hpp>
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+#include <opencv2/cudafeatures2d.hpp>
+#endif
+#endif
+
+#include "rtflann/flann.hpp"
 
 #include <fstream>
 #include <string>
@@ -42,17 +53,246 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class FlannIndex
+{
+public:
+	FlannIndex():
+		index_(0),
+		nextIndex_(0),
+		featuresType_(0),
+		featuresDim_(0),
+		isLSH_(false)
+	{
+	}
+	virtual ~FlannIndex()
+	{
+		this->release();
+	}
+
+	void release()
+	{
+		if(index_)
+		{
+			if(featuresType_ == CV_8UC1)
+			{
+				delete (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+			}
+			else
+			{
+				delete (rtflann::Index<rtflann::L2<float> >*)index_;
+			}
+			index_ = 0;
+		}
+		nextIndex_ = 0;
+		isLSH_ = false;
+		addedDescriptors_.clear();
+		removedIndexes_.clear();
+	}
+
+	unsigned int indexedFeatures() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(featuresType_ == CV_8UC1)
+		{
+			return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->size();
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
+		}
+	}
+
+	// return KB
+	unsigned int memoryUsed() const
+	{
+		if(!index_)
+		{
+			return 0;
+		}
+		if(featuresType_ == CV_8UC1)
+		{	
+			return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+		}
+		else
+		{
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+		}
+	}
+
+	void build(
+			const cv::Mat & features,
+			const rtflann::IndexParams& params)
+	{
+		this->release();
+		UASSERT(index_ == 0);
+		UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
+		featuresType_ = features.type();
+		featuresDim_ = features.cols;
+
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
+			index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, params);
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
+		}
+		else
+		{
+			rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
+			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+		}
+
+		if(features.rows == 1)
+		{
+			// incremental FLANN
+			addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+		}
+		// else assume that the features are kept in memory outside this class (e.g., dataTree_)
+
+		nextIndex_ = features.rows;
+	}
+
+	bool isBuilt()
+	{
+		return index_!=0;
+	}
+
+	int featuresType() const {return featuresType_;}
+	int featuresDim() const {return featuresDim_;}
+
+	unsigned int addPoint(const cv::Mat & feature)
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return 0;
+		}
+		UASSERT(feature.type() == featuresType_);
+		UASSERT(feature.cols == featuresDim_);
+		UASSERT(feature.rows == 1);
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned char> point(feature.data, feature.rows, feature.cols);
+			rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
+			index->addPoints(point, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				// clean not used features
+				for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+				{
+					addedDescriptors_.erase(*iter);
+				}
+				removedIndexes_.clear();
+				index->buildIndex();
+			}
+		}
+		else
+		{
+			rtflann::Matrix<float> point((float*)feature.data, feature.rows, feature.cols);
+			rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
+			index->addPoints(point, 0);
+			// Rebuild index if it doubles in size
+			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			{
+				// clean not used features
+				for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+				{
+					addedDescriptors_.erase(*iter);
+				}
+				removedIndexes_.clear();
+				index->buildIndex();
+			}
+		}
+
+		addedDescriptors_.insert(std::make_pair(nextIndex_, feature));
+
+		return nextIndex_++;
+	}
+
+	void removePoint(unsigned int index)
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return;
+		}
+
+		// If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h":
+		// 707 - if (ids_[id]==id) {
+		// 707 + if (id < ids_.size() && ids_[id]==id) {
+		// ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151
+
+		if(featuresType_ == CV_8UC1)
+		{
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
+		}
+		else
+		{
+			((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
+		}
+		removedIndexes_.push_back(index);
+	}
+
+	void knnSearch(
+			const cv::Mat & query,
+			cv::Mat & indices,
+			cv::Mat & dists,
+	        int knn,
+	        const rtflann::SearchParams& params=rtflann::SearchParams())
+	{
+		if(!index_)
+		{
+			UERROR("Flann index not yet created!");
+			return;
+		}
+		indices.create(query.rows, knn, CV_32S);
+		dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
+
+		rtflann::Matrix<int> indicesF((int*)indices.data, indices.rows, indices.cols);
+
+		if(featuresType_ == CV_8UC1)
+		{
+			rtflann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
+			rtflann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
+			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+		else
+		{
+			rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
+			rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
+			((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+		}
+	}
+
+private:
+	void * index_;
+	unsigned int nextIndex_;
+	int featuresType_;
+	int featuresDim_;
+	bool isLSH_;
+
+	// keep feature in memory until the tree is rebuilt
+	// (in case the word is deleted when removed from the VWDictionary)
+	std::map<int, cv::Mat> addedDescriptors_;
+	std::list<int> removedIndexes_;
+};
+
 const int VWDictionary::ID_START = 1;
 const int VWDictionary::ID_INVALID = 0;
 
 VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_totalActiveReferences(0),
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
+	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_dictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
 	_lastWordId(0),
-	_flannIndex(new cv::flann::Index()),
+	_flannIndex(new FlannIndex()),
 	_strategy(kNNBruteForce)
 {
 	this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
@@ -70,8 +310,9 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
+	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 
-	UASSERT(_nndrRatio > 0.0f);
+	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
 	std::string dictionaryPath = _dictionaryPath;
 	bool incrementalDictionary = _incrementalDictionary;
@@ -167,7 +408,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 							//get descriptor
 							for(;i<dimension && iter != strList.end(); ++i, ++iter)
 							{
-								descriptor.at<float>(i) = std::atof(iter->c_str());
+								descriptor.at<float>(i) = uStr2Float(*iter);
 							}
 							if(i != dimension)
 							{
@@ -209,6 +450,10 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	{
 		_incrementalDictionary = false;
 	}
+	else if(_incrementalDictionary)
+	{
+		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
+	}
 	_dictionaryPath = dictionaryPath;
 }
 
@@ -216,11 +461,26 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 	if(strategy!=kNNUndef)
 	{
+#if CV_MAJOR_VERSION < 3
 		if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
 		{
 			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
 			strategy = kNNBruteForce;
 		}
+#else
+		if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
+		{
+			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
+			strategy = kNNBruteForce;
+		}
+#endif
+#ifndef HAVE_OPENCV_CUDAFEATURES2D
+		if(strategy == kNNBruteForceGPU)
+		{
+			UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
+			strategy = kNNBruteForce;
+		}
+#endif
 
 		if(RTABMAP_NONFREE == 0 && strategy == kNNFlannKdTree)
 		{
@@ -230,7 +490,15 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 		}
 		else
 		{
+			bool update = _strategy != strategy;
 			_strategy = strategy;
+			if(update)
+			{
+				_dataTree = cv::Mat();
+				_notIndexedWords = uKeysSet(_visualWords);
+				_removedIndexedWords.clear();
+				this->update();
+			}
 		}
 	}
 }
@@ -247,6 +515,16 @@ int VWDictionary::getLastIndexedWordId() const
 	}
 }
 
+unsigned int VWDictionary::getIndexedWordsCount() const
+{
+	return _flannIndex->indexedFeatures();
+}
+
+unsigned int VWDictionary::getIndexMemoryUsed() const
+{
+	return _flannIndex->memoryUsed();
+}
+
 void VWDictionary::update()
 {
 	ULOGGER_DEBUG("");
@@ -260,58 +538,144 @@ void VWDictionary::update()
 
 	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
-		_mapIndexId.clear();
-		int oldSize = _dataTree.rows;
-		_dataTree = cv::Mat();
-		_flannIndex->release();
-
-		if(_visualWords.size())
+		if(_incrementalFlann &&
+		   _strategy < kNNBruteForce &&
+		   _visualWords.size())
 		{
-			UTimer timer;
-			timer.start();
-
-			int type = _visualWords.begin()->second->getDescriptor().type();
-			int dim = _visualWords.begin()->second->getDescriptor().cols;
-
-			UASSERT(type == CV_32F || type == CV_8U);
-			UASSERT(dim > 0);
-
-			// Create the data matrix
-			_dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
-			std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
-			for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
+			ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
+			for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
 			{
-				UASSERT(iter->second->getDescriptor().cols == dim);
-				UASSERT(iter->second->getDescriptor().type() == type);
-
-				iter->second->getDescriptor().copyTo(_dataTree.row(i));
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+				UASSERT(uContains(_mapIdIndex, *iter));
+				UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter)));
+				_flannIndex->removePoint(_mapIdIndex.at(*iter));
+				_mapIndexId.erase(_mapIdIndex.at(*iter));
+				_mapIdIndex.erase(*iter);
 			}
+			ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size());
 
-			ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
-			ULOGGER_DEBUG("copying data = %f s", timer.ticks());
-
-			switch(_strategy)
+			if(_notIndexedWords.size())
 			{
-			case kNNFlannNaive:
-				_flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
-				break;
-			case kNNFlannKdTree:
-				UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-				_flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2);
-				break;
-			case kNNFlannLSH:
-				UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-				_flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
-				break;
-			default:
-				break;
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
+				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
+				{
+					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
+					UASSERT(w);
+					int index = 0;
+					if(!_flannIndex->isBuilt())
+					{
+						UDEBUG("Building FLANN index...");
+						switch(_strategy)
+						{
+						case kNNFlannNaive:
+							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams());
+							break;
+						case kNNFlannKdTree:
+							UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams());
+							break;
+						case kNNFlannLSH:
+							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2));
+							break;
+						default:
+							UFATAL("Not supposed to be here!");
+							break;
+						}
+						UDEBUG("Building FLANN index... done!");
+					}
+					else
+					{
+						UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim());
+						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
+						index = _flannIndex->addPoint(w->getDescriptor());
+					}
+					std::pair<std::map<int, int>::iterator, bool> inserted;
+					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
+					UASSERT(inserted.second);
+					inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
+					UASSERT(inserted.second);
+				}
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
 			}
-
-			ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 		}
-		UDEBUG("Dictionary updated! (size=%d->%d added=%d removed=%d)",
-				oldSize, _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
+		else if(_strategy >= kNNBruteForce &&
+				_notIndexedWords.size() &&
+				_removedIndexedWords.size() == 0 &&
+				_visualWords.size() &&
+				_dataTree.rows)
+		{
+			//just add not indexed words
+			int i = _dataTree.rows;
+			_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
+			for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
+			{
+				VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
+				UASSERT(w);
+				UASSERT(w->getDescriptor().cols == _dataTree.cols);
+				UASSERT(w->getDescriptor().type() == _dataTree.type());
+				_dataTree.push_back(w->getDescriptor());
+				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
+				std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
+				UASSERT(inserted.second);
+				++i;
+			}
+		}
+		else
+		{
+			_mapIndexId.clear();
+			_mapIdIndex.clear();
+			_dataTree = cv::Mat();
+			_flannIndex->release();
+
+			if(_visualWords.size())
+			{
+				UTimer timer;
+				timer.start();
+
+				int type = _visualWords.begin()->second->getDescriptor().type();
+				int dim = _visualWords.begin()->second->getDescriptor().cols;
+
+				UASSERT(type == CV_32F || type == CV_8U);
+				UASSERT(dim > 0);
+
+				// Create the data matrix
+				_dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
+				std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
+				for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
+				{
+					UASSERT(iter->second->getDescriptor().cols == dim);
+					UASSERT(iter->second->getDescriptor().type() == type);
+
+					iter->second->getDescriptor().copyTo(_dataTree.row(i));
+					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+					_mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
+				}
+
+				ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
+				ULOGGER_DEBUG("copying data = %f s", timer.ticks());
+
+				switch(_strategy)
+				{
+				case kNNFlannNaive:
+					_flannIndex->build(_dataTree, rtflann::LinearIndexParams());
+					break;
+				case kNNFlannKdTree:
+					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
+					_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams());
+					break;
+				case kNNFlannLSH:
+					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
+					_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2));
+					break;
+				default:
+					break;
+				}
+
+				ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
+			}
+		}
+		UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)",
+				_dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
 	}
 	else
 	{
@@ -319,6 +683,7 @@ void VWDictionary::update()
 	}
 	_notIndexedWords.clear();
 	_removedIndexedWords.clear();
+	UDEBUG("");
 }
 
 void VWDictionary::clear()
@@ -343,6 +708,7 @@ void VWDictionary::clear()
 	_lastWordId = 0;
 	_dataTree = cv::Mat();
 	_mapIndexId.clear();
+	_mapIdIndex.clear();
 	_unusedWords.clear();
 	_flannIndex->release();
 }
@@ -444,7 +810,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 	UTimer timerLocal;
 	timerLocal.start();
 
-	if(!_dataTree.empty() && _dataTree.rows >= (int)k)
+	if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 	{
 		//Find nearest neighbors
 		UDEBUG("newPts.total()=%d ", descriptors.rows);
@@ -456,12 +822,13 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		else if(_strategy == kNNBruteForce)
 		{
 			bruteForce = true;
-			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
+			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 			matcher.knnMatch(descriptors, _dataTree, matches, k);
 		}
 		else if(_strategy == kNNBruteForceGPU)
 		{
 			bruteForce = true;
+#if CV_MAJOR_VERSION < 3
 			cv::gpu::GpuMat newDescriptorsGpu(descriptors);
 			cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
 			if(type==CV_8U)
@@ -474,6 +841,23 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 				cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
 				gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 			}
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+			cv::cuda::GpuMat newDescriptorsGpu(descriptors);
+			cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
+			cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
+			if(type==CV_8U)
+			{
+				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
+				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+			}
+			else
+			{
+				gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
+				gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+			}
+#endif
+#endif
 		}
 		else
 		{
@@ -499,10 +883,15 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		{
 			for(int j=0; j<dists.cols; ++j)
 			{
-				if(results.at<int>(i,j) >= 0)
+				float d = dists.at<float>(i,j);
+				int id = uValue(_mapIndexId, results.at<int>(i,j));
+				if(d >= 0.0f && id > 0)
 				{
-					float d = dists.at<float>(i,j);
-					fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, results.at<int>(i,j))));
+					fullResults.insert(std::pair<float, int>(d, id));
+				}
+				else
+				{
+					break;
 				}
 			}
 		}
@@ -510,10 +899,15 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		{
 			for(unsigned int j=0; j<matches.at(i).size(); ++j)
 			{
-				if(matches.at(i).at(j).trainIdx >= 0)
+				float d = matches.at(i).at(j).distance;
+				int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
+				if(d >= 0.0f && id > 0)
 				{
-					float d = matches.at(i).at(j).distance;
-					fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx)));
+					fullResults.insert(std::pair<float, int>(d, id));
+				}
+				else
+				{
+					break;
 				}
 			}
 		}
@@ -521,27 +915,21 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		// Check if this descriptor matches with a word from the last signature (a word not already added to the tree)
 		if(_newWordsComparedTogether && newWords.rows)
 		{
-			cv::flann::Index linearSeach;
-			linearSeach.build(newWords, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
-			cv::Mat resultsLinear;
-			cv::Mat distsLinear;
-			linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1);
-			// In case of binary descriptors
-			if(distsLinear.type() == CV_32S)
+			std::vector<std::vector<cv::DMatch> > matchesNewWords;
+			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1);
+			UASSERT(matchesNewWords.size() == 1);
+			for(unsigned int j=0; j<matchesNewWords.at(0).size(); ++j)
 			{
-				cv::Mat temp;
-				distsLinear.convertTo(temp, CV_32F);
-				distsLinear = temp;
-			}
-			if(resultsLinear.cols)
-			{
-				for(int j=0; j<resultsLinear.cols; ++j)
+				float d = matchesNewWords.at(0).at(j).distance;
+				int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
+				if(d >= 0.0f && id > 0)
 				{
-					if(resultsLinear.at<int>(0,j) >= 0)
-					{
-						 float d = distsLinear.at<float>(0,j);
-						 fullResults.insert(std::pair<float, int>(d, newWordsId[resultsLinear.at<int>(0,j)]));
-					}
+					fullResults.insert(std::pair<float, int>(d, id));
+				}
+				else
+				{
+					break;
 				}
 			}
 		}
@@ -592,7 +980,6 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 
 				this->addWordRef(fullResults.begin()->second, signatureId);
 				wordIds.push_back(fullResults.begin()->second);
-				UASSERT(fullResults.begin()->second>0);
 			}
 		}
 		else if(fullResults.size())
@@ -659,7 +1046,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());
 
-		if(!_dataTree.empty() && _dataTree.rows >= (int)k)
+		if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 		{
 			//Find nearest neighbors
 			UDEBUG("newPts.total()=%d ", query.total());
@@ -671,12 +1058,13 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			else if(_strategy == kNNBruteForce)
 			{
 				bruteForce = true;
-				cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
+				cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
 				matcher.knnMatch(query, _dataTree, matches, k);
 			}
 			else if(_strategy == kNNBruteForceGPU)
 			{
 				bruteForce = true;
+#if CV_MAJOR_VERSION < 3
 				cv::gpu::GpuMat newDescriptorsGpu(query);
 				cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
 				if(type==CV_8U)
@@ -689,6 +1077,23 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 					cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
 					gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
 				}
+#else
+#ifdef HAVE_OPENCV_CUDAFEATURES2D
+				cv::cuda::GpuMat newDescriptorsGpu(query);
+				cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
+				cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
+				if(type==CV_8U)
+				{
+					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
+					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				}
+				else
+				{
+					gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
+					gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
+				}
+#endif
+#endif
 			}
 			else
 			{
@@ -705,9 +1110,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Search dictionary time = %fs", timer.ticks());
 
-		cv::Mat resultsNotIndexed;
-		cv::Mat distsNotIndexed;
 		std::map<int, int> mapIndexIdNotIndexed;
+		std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
 		if(_notIndexedWords.size())
 		{
 			cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type);
@@ -723,16 +1127,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 
 			// Find nearest neighbor
 			ULOGGER_DEBUG("Searching in words not indexed...");
-			cv::flann::Index linearSeach;
-			linearSeach.build(dataNotIndexed, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
-			linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1);
-			// In case of binary descriptors
-			if(distsNotIndexed.type() == CV_32S)
-			{
-				cv::Mat temp;
-				distsNotIndexed.convertTo(temp, CV_32F);
-				distsNotIndexed = temp;
-			}
+			cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+			matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1);
 		}
 		ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());
 
@@ -743,10 +1139,11 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			{
 				for(int j=0; j<dists.cols; ++j)
 				{
-					if(results.at<int>(i,j) > 0)
+					float d = dists.at<float>(i,j);
+					int id = uValue(_mapIndexId, results.at<int>(i,j));
+					if(d >= 0.0f && id > 0)
 					{
-						float d = dists.at<float>(i,j);
-						fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, results.at<int>(i,j))));
+						fullResults.insert(std::pair<float, int>(d, id));
 					}
 				}
 			}
@@ -754,21 +1151,30 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			{
 				for(unsigned int j=0; j<matches.at(i).size(); ++j)
 				{
-					if(matches.at(i).at(j).trainIdx > 0)
+					float d = matches.at(i).at(j).distance;
+					int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
+					if(d >= 0.0f && id > 0)
 					{
-						float d = matches.at(i).at(j).distance;
-						fullResults.insert(std::pair<float, int>(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx)));
+						fullResults.insert(std::pair<float, int>(d, id));
 					}
 				}
 			}
 
 			// not indexed..
-			for(int j=0; j<distsNotIndexed.cols; ++j)
+			if(matchesNotIndexed.size())
 			{
-				if(resultsNotIndexed.at<int>(i,j) > 0)
+				for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
 				{
-					float d = distsNotIndexed.at<float>(i,j);
-					fullResults.insert(std::pair<float, int>(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at<int>(i,j))));
+					float d = matchesNotIndexed.at(i).at(j).distance;
+					int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
+					if(d >= 0.0f && id > 0)
+					{
+						fullResults.insert(std::pair<float, int>(d, id));
+					}
+					else
+					{
+						break;
+					}
 				}
 			}