Increased ROS package version to 0.10.4

Merging devel to master

CMakeLists.txt

@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 9)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_MINOR_VERSION 10)
+SET(RTABMAP_PATCH_VERSION 4)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -122,11 +122,6 @@ ENDIF(APPLE)
 
 ####### DEPENDENCIES #######
 FIND_PACKAGE(OpenCV REQUIRED)
-IF(OPENCV_NONFREE_FOUND)
-  SET(NONFREE 1)
-ELSE()
-  SET(NONFREE 0)
-ENDIF()
 FIND_PACKAGE(PCL 1.7 REQUIRED)
 FIND_PACKAGE(VTK REQUIRED)
 IF("${VTK_MAJOR_VERSION}" EQUAL 5)
@@ -176,6 +171,10 @@ 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 )
@@ -277,10 +276,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")
@@ -328,54 +328,64 @@ 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")
 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")
 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")
+MESSAGE(STATUS "  With Freenect2            = YES")
 ELSE()
-MESSAGE(STATUS "  With Freenect2 = NO (libfreenect2 not found)")
+MESSAGE(STATUS "  With Freenect2            = NO (libfreenect2 not found)")
 ENDIF()
 
 IF(DC1394_FOUND)
-MESSAGE(STATUS "  With dc1394  = YES")
+MESSAGE(STATUS "  With dc1394               = YES")
 ELSE()
-MESSAGE(STATUS "  With dc1394  = NO (dc1394 not found)")
+MESSAGE(STATUS "  With dc1394               = NO (dc1394 not found)")
 ENDIF()
 
 IF(FlyCapture2_FOUND)
-MESSAGE(STATUS "  With FlyCapture2/Triclops  = YES")
+MESSAGE(STATUS "  With FlyCapture2/Triclops = YES")
 ELSE()
-MESSAGE(STATUS "  With FlyCapture2/Triclops  = NO (Point Grey SDK not found)")
+MESSAGE(STATUS "  With FlyCapture2/Triclops = NO (Point Grey SDK not found)")
 ENDIF()
 
 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o  = YES")
+MESSAGE(STATUS "  With g2o                  = YES")
 ELSE()
-MESSAGE(STATUS "  With g2o  = NO (g2o not found)")
+MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
 ENDIF()
 
 IF(QT4_FOUND)
-MESSAGE(STATUS "  With Qt  = YES (version 4)")
+MESSAGE(STATUS "  With Qt                   = YES (version 4)")
 ELSEIF(Qt5_FOUND)
-MESSAGE(STATUS "  With Qt  = YES (version 5)")
+MESSAGE(STATUS "  With Qt                   = YES (version 5)")
 ELSE()
-MESSAGE(STATUS "  With Qt  = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)")
+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/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))
 %% -------------------------

cmake_modules/FindFlyCapture2.cmake

@@ -6,15 +6,27 @@
 #  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 PATHS $ENV{FlyCapture2_ROOT_DIR}/lib64 $ENV{FlyCapture2_ROOT_DIR}/lib)
+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 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
-find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
+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)
@@ -22,6 +34,9 @@ IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND
    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)
 
+MESSAGE(STATUS "FlyCapture2_INCLUDE_DIRS={FlyCapture2_INCLUDE_DIRS}")
+MESSAGE(STATUS "FlyCapture2_LIBRARIES={FlyCapture2_LIBRARIES}")
+
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet
    IF (NOT FlyCapture2_FIND_QUIETLY)

corelib/include/rtabmap/core/Camera.h

@@ -50,117 +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;}
-	bool isMirroringEnabled() const {return _mirroring;}
+	const Transform & getLocalTransform() const {return _localTransform;}
 
 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setImageSize(unsigned int width, unsigned int height);
-	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
-
-	void setCalibration(const std::string & fileName);
-	void setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients);
-	void resetCalibration();
+	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;
-	bool _mirroring;
+	Transform _localTransform;
+	cv::Size _targetImageSize;
 	UTimer * _frameRateTimer;
-	cv::Mat _k; // camera_matrix
-	cv::Mat _d; // distorsion_coefficients
-};
-
-
-/////////////////////////
-// 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,14 +38,13 @@ class CameraEvent :
 {
 public:
 	enum Code {
-		kCodeImage,
-		kCodeImageDepth,
+		kCodeData,
 		kCodeNoMoreImages
 	};
 
 public:
 	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
-		UEvent(kCodeImage),
+		UEvent(kCodeData),
 		data_(image, seq, stamp),
 		cameraName_(cameraName)
 	{
@@ -57,7 +56,7 @@ public:
 	}
 
 	CameraEvent(const SensorData & data, const std::string & cameraName = "") :
-		UEvent(kCodeImageDepth),
+		UEvent(kCodeData),
 		data_(data),
 		cameraName_(cameraName)
 	{

corelib/include/rtabmap/core/CameraModel.h

@@ -43,17 +43,43 @@ public:
 	// 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);
+	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() &&
-									imageSize_.height &&
-									imageSize_.width &&
-									!name_.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);}
@@ -67,12 +93,17 @@ public:
 	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_;}
+
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageWeight() const {return imageSize_.height;}
 
-	bool load(const std::string & filePath);
-	bool save(const std::string & filePath);
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory) const;
+
+	void scale(double scale);
 
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
@@ -87,20 +118,23 @@ private:
 	cv::Mat P_;
 	cv::Mat mapX_;
 	cv::Mat mapY_;
+	Transform localTransform_;
 };
 
 class RTABMAP_EXP StereoCameraModel
 {
 public:
 	StereoCameraModel() {}
-	StereoCameraModel(const std::string & name,
+	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) :
-		left_(name+"_left", imageSize1, K1, D1, R1, P1),
-		right_(name+"_right", imageSize2, K2, D2, R2, 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),
@@ -108,13 +142,41 @@ public:
 		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() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
+	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 save(const std::string & directory, const std::string & cameraName);
+	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();}
 
@@ -123,7 +185,11 @@ public:
 	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
 	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
 
-	Transform transform() const;
+	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_;}

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,24 +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 "rtabmap/core/CameraModel.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
+#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;
@@ -67,73 +59,20 @@ class Registration;
 class PacketPipeline;
 }
 
-namespace FlyCapture2
-{
-class Camera;
-}
-
 typedef struct _freenect_context freenect_context;
 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(const std::string & calibrationFolder = ".") = 0;
-	virtual bool isCalibrated() const = 0;
-	virtual std::string getSerial() const = 0;
-
-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-	bool isMirroringEnabled() const {return _mirroring;}
-	bool isColorOnly() const {return _colorOnly;}
-
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
-	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
-
-protected:
-	/**
-	 * Constructor
-	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
-	 */
-	CameraRGBD(float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity());
-
-	/**
-	 * returned rgb and depth images should be already rectified
-	 */
-	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy) = 0;
-
-private:
-	float _imageRate;
-	Transform _localTransform;
-	bool _mirroring;
-	bool _colorOnly;
-	UTimer * _frameRateTimer;
-};
-
 /////////////////////////
 // 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));
@@ -141,18 +80,19 @@ public:
 			float imageRate = 0,
 			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
 
-    virtual bool init(const std::string & calibrationFolder = ".");
+    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_;
@@ -169,7 +109,7 @@ private:
 // CameraOpenNICV
 /////////////////////////
 class RTABMAP_EXP CameraOpenNICV :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -181,12 +121,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNICV();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	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;
@@ -198,7 +138,7 @@ private:
 // CameraOpenNI2
 /////////////////////////
 class RTABMAP_EXP CameraOpenNI2 :
-	public CameraRGBD
+	public Camera
 {
 
 public:
@@ -211,7 +151,7 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
@@ -222,7 +162,7 @@ public:
 	bool setMirroring(bool enabled);
 
 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;
@@ -240,7 +180,7 @@ private:
 class FreenectDevice;
 
 class RTABMAP_EXP CameraFreenect :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available();
@@ -252,12 +192,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	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_;
@@ -270,7 +210,7 @@ private:
 /////////////////////////
 
 class RTABMAP_EXP CameraFreenect2 :
-	public CameraRGBD
+	public Camera
 {
 public:
 	static bool available();
@@ -290,12 +230,12 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect2();
 
-	virtual bool init(const std::string & calibrationFolder = ".");
+	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_;
@@ -308,56 +248,44 @@ private:
 	libfreenect2::Registration * reg_;
 };
 
-/////////////////////////
-// CameraStereoDC1394
-/////////////////////////
-class DC1394Device;
 
-class RTABMAP_EXP CameraStereoDC1394 :
-	public CameraRGBD
+/////////////////////////
+// CameraRGBDImages
+/////////////////////////
+class CameraImages;
+class RTABMAP_EXP CameraRGBDImages :
+	public Camera
 {
 public:
 	static bool available();
 
 public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoDC1394();
+	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 = ".");
+	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 & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
+	virtual SensorData captureImage();
 
 private:
-	DC1394Device *device_;
-	StereoCameraModel stereoModel_;
-};
-
-/////////////////////////
-// CameraStereoFlyCapture2
-/////////////////////////
-class RTABMAP_EXP CameraStereoFlyCapture2 :
-	public CameraRGBD
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoFlyCapture2();
-
-	virtual bool init(const std::string & calibrationFolder = ".");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
-
-private:
-	FlyCapture2::Camera * camera_;
-	void * triclopsCtx_; // TriclopsContext
+	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,10 @@ 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;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -60,15 +59,15 @@ 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;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/DBDriver.h

@@ -38,6 +38,7 @@ 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>
@@ -84,6 +85,7 @@ public:
 	void closeConnection();
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
+	std::string getDatabaseVersion() const;
 
 	void executeNoResult(const std::string & sql) const;
 
@@ -94,13 +96,13 @@ 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 & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const;
-	void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) 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;
@@ -115,6 +117,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;
 
@@ -133,11 +136,10 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) 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 & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) 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;

corelib/include/rtabmap/core/DBReader.h

@@ -34,7 +34,7 @@ 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/SensorData.h>
+#include <rtabmap/core/OdometryEvent.h>
 
 #include <opencv2/core/core.hpp>
 
@@ -59,7 +59,7 @@ public:
 
 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	SensorData getNextData();
+	OdometryEvent getNextData();
 
 protected:
 	virtual void mainLoopBegin();

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 {
 
@@ -134,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
@@ -159,7 +198,7 @@ private:
 	double edgeThreshold_;
 	double sigma_;
 
-	cv::SIFT * _sift;
+	cv::Ptr<CV_SIFT> _sift;
 };
 
 //ORB
@@ -190,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
@@ -212,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
@@ -232,7 +271,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //FAST_FREAK
@@ -254,7 +293,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //GFTT
@@ -277,7 +316,7 @@ private:
 	bool _useHarrisDetector;
 	double _k;
 
-	cv::GFTTDetector * _gftt;
+	cv::Ptr<CV_GFTT> _gftt;
 };
 
 //GFTT_BRIEF
@@ -296,7 +335,7 @@ private:
 private:
 	int bytes_;
 
-	cv::BriefDescriptorExtractor * _brief;
+	cv::Ptr<CV_BRIEF> _brief;
 };
 
 //GFTT_FREAK
@@ -318,7 +357,7 @@ private:
 	float patternScale_;
 	int nOctaves_;
 
-	cv::FREAK * _freak;
+	cv::Ptr<CV_FREAK> _freak;
 };
 
 //BRISK
@@ -340,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

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 
 namespace rtabmap {
+class Memory;
 
 namespace graph {
 
@@ -70,6 +71,7 @@ public:
 	int iterations() const {return iterations_;}
 	bool isSlam2d() const {return slam2d_;}
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
+	double epsilon() const {return epsilon_;}
 
 	virtual std::map<int, Transform> optimize(
 			int rootId,
@@ -80,13 +82,18 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 
 protected:
-	Optimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false);
+	Optimizer(
+			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
+			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
+			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
+			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon());
 	Optimizer(const ParametersMap & parameters);
 
 private:
 	int iterations_;
 	bool slam2d_;
 	bool covarianceIgnored_;
+	double epsilon_;
 };
 
 class RTABMAP_EXP TOROOptimizer : public Optimizer
@@ -149,6 +156,14 @@ 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.
@@ -192,6 +207,22 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 			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);
+
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose);

corelib/include/rtabmap/core/Link.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -42,19 +43,33 @@ public:
 		from_(0),
 		to_(0),
 		type_(kUndef),
-		rotVariance_(1.0f),
-		transVariance_(1.0f)
+		infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
-	Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) :
 		from_(from),
 		to_(to),
 		transform_(transform),
-		type_(type),
-		rotVariance_(rotVariance),
-		transVariance_(transVariance)
+		type_(type)
 	{
-		UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+		setInfMatrix(infMatrix);
+	}
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			double rotVariance,
+			double transVariance) :
+		from_(from),
+		to_(to),
+		transform_(transform),
+		type_(type)
+	{
+		setVariance(rotVariance, transVariance);
 	}
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -63,17 +78,65 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
-	float rotVariance() const {return rotVariance_;}
-	float transVariance() const {return transVariance_;}
+	const cv::Mat & infMatrix() const {return infMatrix_;}
+	double 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 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 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 setVariance(float rotVariance, float transVariance) {
-		UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
-		rotVariance_ = rotVariance;
-		transVariance_ = transVariance;
+	void 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 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;
+	}
+
+	Link merge(const Link & link) const
+	{
+		UASSERT(to_ == link.from());
+		UASSERT(type_ == link.type());
+		UASSERT(!transform_.isNull());
+		UASSERT(!link.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(),
+				type_,
+				transform_ * link.transform(),
+				infMatrix_ + link.infMatrix());
+	}
+
+	Link inverse() const
+	{
+		return Link(to_, from_, type_, transform_.inverse(), infMatrix_);
 	}
 
 private:
@@ -81,8 +144,7 @@ private:
 	int to_;
 	Transform transform_;
 	Type type_;
-	float rotVariance_;
-	float transVariance_;
+	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -42,6 +42,7 @@ 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 {
 
@@ -65,7 +66,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(),
@@ -78,11 +84,12 @@ public:
 	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 addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
+	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(
@@ -91,6 +98,7 @@ public:
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
+			bool ignoreIntermediateNodes = false,
 			double * dbAccessTime = 0) const;
 	std::map<int, float> getNeighborsIdRadius(
 			int signatureId,
@@ -108,6 +116,9 @@ public:
 			bool lookInDatabase = false) const;
 	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
+	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;}
@@ -117,8 +128,9 @@ public:
 	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 std::vector<unsigned char> & data);
+	bool setUserData(int id, const cv::Mat & data);
 	int getDatabaseMemoryUsed() const; // in bytes
+	std::string getDatabaseVersion() const;
 	double getDbSavingTime() const;
 	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
 	bool getNodeInfo(int signatureId,
@@ -127,11 +139,13 @@ public:
 			int & weight,
 			std::string & label,
 			double & stamp,
-			std::vector<unsigned char> & userData,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	Signature getSignatureData(int locationId, bool uncompressedData = false);
-	Signature getSignatureDataConst(int locationId) const;
+	SensorData getNodeData(int nodeId, bool uncompressedData = false);
+	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;}
@@ -168,7 +182,6 @@ public:
 	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, double * variance = 0) const;
 	Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
@@ -184,7 +197,7 @@ public:
 
 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
+	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
 	void clear();
 	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
@@ -202,6 +215,7 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
+			const Transform & pose,
 			Statistics * stats = 0);
 
 	//keypoint stuff
@@ -260,10 +274,12 @@ private:
 	int _bowMinInliers;
 	float _bowInlierDistance;
 	int _bowIterations;
-	float _bowMaxDepth;
+	int _bowRefineIterations;
 	bool _bowForce2D;
-	bool _bowEpipolarGeometry;
 	float _bowEpipolarGeometryVar;
+	int _bowEstimationType;
+	double _bowPnPReprojError;
+	int _bowPnPFlags;
 	float _icpMaxTranslation;
 	float _icpMaxRotation;
 	int _icpDecimation;

corelib/include/rtabmap/core/Odometry.h

@@ -42,11 +42,12 @@ namespace rtabmap {
 
 class Feature2D;
 class OdometryInfo;
+class ParticleFilter;
 
 class RTABMAP_EXP Odometry
 {
 public:
-	virtual ~Odometry() {}
+	virtual ~Odometry();
 	Transform process(const SensorData & data, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
@@ -59,9 +60,10 @@ public:
 	int getRefineIterations() const {return _refineIterations;}
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
-	bool isPnPEstimationUsed() const {return _pnpEstimation;}
+	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, OdometryInfo * info = 0) = 0;
@@ -75,12 +77,24 @@ private:
 	float _maxDepth;
 	int _resetCountdown;
 	bool _force2D;
+	bool _holonomic;
+	bool _particleFiltering;
+	int _particleSize;
+	float _particleNoiseT;
+	float _particleLambdaT;
+	float _particleNoiseR;
+	float _particleLambdaR;
 	bool _fillInfoData;
-	bool _pnpEstimation;
+	int _estimationType;
 	double _pnpReprojError;
 	int _pnpFlags;
 	Transform _pose;
 	int _resetCurrentCount;
+	double previousStamp_;
+	Transform previousTransform_;
+	float distanceTravelled_;
+
+	std::vector<ParticleFilter *> filters_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);
@@ -95,7 +109,7 @@ 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:
@@ -104,9 +118,10 @@ private:
 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
@@ -123,9 +138,7 @@ public:
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
-	Transform computeTransformStereo(const SensorData & image, OdometryInfo * info);
-	Transform computeTransformRGBD(const SensorData & image, OdometryInfo * info);
-	Transform computeTransformMono(const SensorData & image, OdometryInfo * info);
+
 private:
 	//Parameters:
 	int flowWinSize_;
@@ -146,7 +159,6 @@ private:
 	Feature2D * feature2D_;
 
 	cv::Mat refFrame_;
-	cv::Mat refRightFrame_;
 	std::vector<cv::Point2f> refCorners_;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
 };
@@ -167,6 +179,12 @@ private:
 	double flowEps_;
 	int flowMaxLevel_;
 
+	int stereoWinSize_;
+	int stereoIterations_;
+	double stereoEps_;
+	int stereoMaxLevel_;
+	float stereoMaxSlope_;
+
 	Memory * memory_;
 	int localHistoryMaxSize_;
 	float initMinFlow_;
@@ -175,9 +193,9 @@ private:
 	float fundMatrixReprojError_;
 	float fundMatrixConfidence_;
 
-	cv::Mat refDepth_;
+	cv::Mat refDepthOrRight_;
 	std::map<int, cv::Point2f> cornersMap_;
-	std::multimap<int, cv::Point3f> localMap_;
+	std::map<int, cv::Point3f> localMap_;
 	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
 	float maxVariance_;

corelib/include/rtabmap/core/OdometryEvent.h

@@ -29,6 +29,8 @@ 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"
 
@@ -37,20 +39,69 @@ 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, const OdometryInfo & info = OdometryInfo()) :
+		const SensorData & data,
+		const Transform & pose,
+		const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1),
+		const OdometryInfo & info = OdometryInfo()) :
 			_data(data),
+			_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;}
+	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;
+	Transform _pose;
+	cv::Mat _covariance;
 	OdometryInfo _info;
 };
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ODOMETRYINFO_H_
 #define ODOMETRYINFO_H_
 
+#include <map>
+
 namespace rtabmap {
 
 class OdometryInfo
@@ -40,7 +42,10 @@ public:
 		variance(-1),
 		features(-1),
 		localMapSize(-1),
-		time(-1),
+		timeEstimation(-1),
+		stamp(0),
+		interval(0),
+		distanceTravelled(0),
 		type(-1)
 	{}
 	bool lost;
@@ -49,7 +54,13 @@ public:
 	float variance;
 	int features;
 	int localMapSize;
-	float time;
+	float timeEstimation;
+	float timeParticleFiltering;
+	double stamp;
+	double interval;
+	Transform transform;
+	Transform transformFiltered;
+	float distanceTravelled;
 
 	int type; // 0=BOW, 1=Optical Flow, 2=ICP
 
@@ -57,7 +68,7 @@ public:
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
-	std::multimap<int, cv::Point3f> localMap;
+	std::map<int, cv::Point3f> localMap;
 
 	// Optical Flow odometry
 	std::vector<cv::Point2f> refCorners;

corelib/include/rtabmap/core/OdometryThread.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
+#include <list>
 
 namespace rtabmap {
 
@@ -40,7 +41,7 @@ class Odometry;
 class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
 public:
 	// take ownership of Odometry
-	OdometryThread(Odometry * odometry);
+	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
 	virtual ~OdometryThread();
 
 protected:
@@ -54,13 +55,14 @@ private:
 	//============================================================
 	void mainLoop();
 	void addData(const SensorData & data);
-	void getData(SensorData & data);
+	bool getData(SensorData & data);
 
 private:
 	USemaphore _dataAdded;
 	UMutex _dataMutex;
-	SensorData _dataBuffer;
+	std::list<SensorData> _dataBuffer;
 	Odometry * _odometry;
+	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 };
 

corelib/include/rtabmap/core/Parameters.h

@@ -169,7 +169,8 @@ 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.");
@@ -290,7 +291,7 @@ class RTABMAP_EXP Parameters
 	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, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
-	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "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, 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.");
@@ -307,28 +308,38 @@ class RTABMAP_EXP Parameters
 	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.001,  "Stop optimizing when the error improvement is less than this value.");
 
 	// 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, EstimationType,  		int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
 	RTABMAP_PARAM(Odom, MaxFeatures,            int, 400, 		"0 no limits.");
 	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
 	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, 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, 4.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, 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, PnPEstimation, 		    bool, false,     "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
-	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 8.0,     "PnP reprojection error.");
-	RTABMAP_PARAM(Odom, PnPFlags, 				int, 0,    	      "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Odom, ImageBufferSize,        unsigned int, 1, "Data buffer size (0 min inf).");
+	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.");
@@ -352,18 +363,21 @@ class RTABMAP_EXP Parameters
 	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, EstimationType,  int, 0,    	    "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
 	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, 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, Force2D, 		   bool, false,    "Force 2D transform (3Dof: x,y and yaw).");
-	RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false,   "Use epipolar geometry to compute the loop closure transform.");
+	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_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.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, 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.");
@@ -371,13 +385,13 @@ class RTABMAP_EXP Parameters
 	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, 		"Max iterations.");
-	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.0, 	"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.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, CorrespondenceRatio, float, 0.0, 		"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

corelib/include/rtabmap/core/Rtabmap.h

@@ -66,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 = "");
@@ -103,9 +106,14 @@ public:
 
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
-	bool setUserData(int id, const std::vector<unsigned char> & data);
+	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 type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO
+	);
 	void resetMemory();
 	void dumpPrediction() const;
 	void dumpData() const;
@@ -115,23 +123,16 @@ public:
 	void get3DMap(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,
 			bool optimized,
 			bool global) const;
 	void getGraph(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,
 			bool optimized,
-			bool global);
+			bool global,
+			std::map<int, Signature> * signatures = 0);
 	void clearPath();
 	bool computePath(int targetNode, bool global);
-	bool computePath(const Transform & targetPose, 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;
@@ -163,7 +164,7 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishLastSignature;
+	bool _publishLastSignatureData;
 	bool _publishPdf;
 	bool _publishLikelihood;
 	float _maxTimeAllowed; // in ms
@@ -195,6 +196,7 @@ private:
 	float _reextractNNDR;
 	int _reextractFeatureType;
 	int _reextractMaxWords;
+	float _reextractMaxDepth;
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planVirtualLinks;

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,49 +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=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO)
 			kCmdCleanDataBuffer,
-			kCmdPublish3DMapLocal, // params: optimized
-			kCmdPublish3DMapGlobal, // params: optimized
-			kCmdPublishTOROGraphGlobal, // params: optimized
-			kCmdPublishTOROGraphLocal, // params: optimized
+			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
 			kCmdTriggerNewMap,
 			kCmdPause,
-			kCmdGoal}; // params: label or location ID
+			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
@@ -148,19 +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::map<int, double> & stamps,
-			const std::map<int, std::string> & labels,
-			const std::map<int, std::vector<unsigned char> > & userDatas) :
+			const std::multimap<int, Link> & constraints) :
 		UEvent(0),
 		_signatures(signatures),
 		_poses(poses),
-		_constraints(constraints),
-		_mapIds(mapIds),
-		_stamps(stamps),
-		_labels(labels),
-		_userDatas(userDatas)
+		_constraints(constraints)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -168,10 +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;}
-	const std::map<int, double> & getStamps() const {return _stamps;}
-	const std::map<int, std::string> & getLabels() const {return _labels;}
-	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
 
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
@@ -179,10 +192,6 @@ private:
 	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;
 };
 
 class RtabmapGlobalPathEvent : public UEvent
@@ -203,6 +212,22 @@ private:
 	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;
+};
+
 } // namespace rtabmap
 
 #endif /* RTABMAPEVENT_H_ */

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,18 +61,15 @@ public:
 		kStateChangingParameters,
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
-		kStateGeneratingDOTGraph,
-		kStateGeneratingDOTLocalGraph,
-		kStateGeneratingTOROGraphLocal,
-		kStateGeneratingTOROGraphGlobal,
+		kStateExportingDOTGraph,
+		kStateExportingPoses,
 		kStateCleanDataBuffer,
-		kStatePublishingMapLocal,
-		kStatePublishingMapGlobal,
-		kStatePublishingTOROGraphLocal,
-		kStatePublishingTOROGraphGlobal,
+		kStatePublishingMap,
 		kStateTriggeringMap,
 		kStateAddingUserData,
-		kStateSettingGoal
+		kStateSettingGoal,
+		kStateCancellingGoal,
+		kStateLabelling
 	};
 
 public:
@@ -81,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);
@@ -90,32 +89,31 @@ 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_;
-	float _rotVariance;
-	float _transVariance;
+	double _rotVariance;
+	double _transVariance;
 
-	std::vector<unsigned char> _userData;
+	cv::Mat _userData;
 	UMutex _userDataMutex;
 };
 

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,71 +44,139 @@ namespace rtabmap
 class RTABMAP_EXP SensorData
 {
 public:
-	SensorData(); // empty constructor
-	SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+	// empty constructor
+	SensorData();
 
-	// Metric constructor
-	SensorData(const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+	// Appearance-only constructor
+	SensorData(
+			const cv::Mat & image,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
 
-	// Metric constructor + 2d laser scan
-	SensorData(const cv::Mat & laserScan,
+	// 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,
-		  const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
+			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,
+			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,
+			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;}
-
-	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
-	void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
-	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 & laserScan() const {return _laserScan;}
 	int laserScanMaxPts() const {return _laserScanMaxPts;}
-	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;}
-	float poseRotVariance() const {return _poseRotVariance;}
-	float poseTransVariance() const {return _poseTransVariance;}
+
+	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 laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;}
+	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)
 	{
@@ -116,33 +186,29 @@ public:
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 	const cv::Mat & descriptors() const {return _descriptors;}
 
-	void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
-	const std::vector<unsigned char> & userData() const {return _userData;}
-
 private:
-	cv::Mat _image;
 	int _id;
 	double _stamp;
-
-	// Metric stuff
-	cv::Mat _depthOrRightImage;
-	cv::Mat _laserScan;
-	float _fx;
-	float _fyOrBaseline;
-	float _cx;
-	float _cy;
-	Transform _pose;
-	Transform _localTransform;
-	float _poseRotVariance;
-	float _poseTransVariance;
 	int _laserScanMaxPts;
 
+	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;
 	cv::Mat _descriptors;
-
-	// user data
-	std::vector<unsigned char> _userData;
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -53,23 +53,12 @@ class RTABMAP_EXP Signature
 public:
 	Signature();
 	Signature(int id,
-			int mapId,
-			int weight,
-			double stamp,
-			const std::string & label,
-			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 std::vector<unsigned char> & userData = std::vector<unsigned char>(),
-			const cv::Mat & laserScan = 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(),
-			int laserScanMaxPts = 0);
+			const SensorData & sensorData = SensorData());
 	virtual ~Signature();
 
 	/**
@@ -87,9 +76,6 @@ public:
 	void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
 	const std::string & getLabel() const {return _label;}
 
-	void setUserData(const std::vector<unsigned char> & data);
-	const std::vector<unsigned char> & getUserData() const {return _userData;}
-
 	double getStamp() const {return _stamp;}
 
 	void addLinks(const std::list<Link> & links);
@@ -121,41 +107,17 @@ 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 setLaserScanCompressed(const cv::Mat & bytes, int maxPts) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
-	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 & getLaserScanCompressed() const {return _laserScanCompressed;}
-	RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead.");
-	RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead.");
-	RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead.");
-	RTABMAP_DEPRECATED(float getDepthCy() const, "Use getCy() instead.");
-	float getFx() const {return _fx;}
-	float getFy() const {return _fy;}
-	float getCx() const {return _cx;}
-	float getCy() const {return _cy;}
-	const Transform & getPose() const {return _pose;}
-	void getPoseVariance(float & rotVariance, float & transVariance) const;
-	const Transform & getLocalTransform() const {return _localTransform;}
-	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
-	const cv::Mat & getDepthRaw() const {return _depthRaw;}
-	void setLaserScanRaw(const cv::Mat & depth2D, int maxPts) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
-	const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
-	int getLaserScanMaxPts() const {return _laserScanMaxPts;}
 
-	SensorData toSensorData();
-	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw);
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) 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;
@@ -164,7 +126,6 @@ private:
 	std::map<int, Link> _links; // id, transform
 	int _weight;
 	std::string _label;
-	std::vector<unsigned char> _userData;
 	bool _saved; // If it's saved to bd
 	bool _modified;
 	bool _linksModified; // Optimization when updating signatures in database
@@ -173,24 +134,13 @@ private:
 	// 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 _laserScanCompressed; // 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>
-	int _laserScanMaxPts;
 
-	cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
-	cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _laserScanRaw; // CV_32FC2
+	SensorData _sensorData;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Statistics.h

@@ -136,11 +136,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 setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
-	void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
-	void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
-	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;}
@@ -159,11 +155,7 @@ public:
 	int loopClosureId() const {return _loopClosureId;}
 	int localLoopClosureId() const {return _localLoopClosureId;}
 
-	const std::map<int, int> & getMapIds() const {return _mapIds;}
-	const std::map<int, std::string> & getLabels() const {return _labels;}
-	const std::map<int, double> & getStamps() const {return _stamps;}
-	const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
-	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;}
@@ -185,14 +177,7 @@ private:
 	int _loopClosureId;
 	int _localLoopClosureId;
 
-	// extended data start here...
-	std::map<int, int> _mapIds;
-	std::map<int, std::string> _labels;
-	std::map<int, double> _stamps;
-	std::map<int, std::vector<unsigned char> > _userDatas;
-
-	// Signature data
-	Signature _signature;
+	std::map<int, Signature> _signatures;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;

corelib/include/rtabmap/core/Transform.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <opencv2/core/core.hpp>
 
 namespace rtabmap {
 
@@ -46,25 +47,27 @@ 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);
 
-	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;
@@ -72,16 +75,16 @@ 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;
 
@@ -121,7 +124,7 @@ public:
 	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

@@ -40,17 +40,17 @@ namespace rtabmap
 class UserDataEvent : public UEvent
 {
 public:
-	UserDataEvent(const std::vector<unsigned char> & data) :
+	UserDataEvent(const cv::Mat & data) :
 		UEvent(0),
 		data_(data)
 	{}
 	~UserDataEvent() {}
 	virtual std::string getClassName() const {return "UserDataEvent";}
 
-	const std::vector<unsigned char> & data() const {return data_;}
+	const cv::Mat & data() const {return data_;}
 
 private:
-	std::vector<unsigned char> data_;
+	cv::Mat data_;
 };
 
 }

corelib/include/rtabmap/core/util3d.h

@@ -34,7 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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
@@ -103,6 +105,38 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		float fx, float baseline,
 		int decimation = 1);
 
+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);
+
+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 cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
+
+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,
 		float disparity,

corelib/include/rtabmap/core/util3d_correspondences.h

@@ -55,7 +55,15 @@ void RTABMAP_EXP findCorrespondences(
 		pcl::PointCloud<pcl::PointXYZ> & inliers1,
 		pcl::PointCloud<pcl::PointXYZ> & inliers2,
 		float maxDepth,
-		std::set<int> * uniqueCorrespondences = 0);
+		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,

corelib/include/rtabmap/core/util3d_features.h

@@ -34,7 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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
 {
@@ -46,20 +48,17 @@ namespace util3d
 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);
+		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,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform);
+		const StereoCameraModel & stereoCameraMode);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		const std::vector<cv::KeyPoint> & keypoints,
@@ -69,24 +68,35 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform = Transform::getIdentity(),
+		Transform localTransform = Transform::getIdentity(),
 		int flowWinSize = 9,
 		int flowMaxLevel = 4,
 		int flowIterations = 20,
-		double flowEps = 0.02);
+		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,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
+		const CameraModel & cameraModel,
 		Transform & cameraTransform,
 		int pnpIterations = 100,
 		float pnpReprojError = 8.0f,
-		int pnpFlags = cv::ITERATIVE,
+		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>(),
@@ -96,14 +106,6 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
-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);
-
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_filtering.h

@@ -115,6 +115,33 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		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.
  */

corelib/include/rtabmap/core/util3d_mapping.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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>

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

@@ -56,32 +56,42 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		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 = 0,
-		double * variance = 0,
-		int * correspondences = 0);
+		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 = 0,
-		double * variance = 0,
-		int * correspondences = 0);
+		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 = 0,
-		double * variance = 0,
-		int * correspondences = 0);
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,

corelib/src/BayesFilter.cpp

@@ -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();)
@@ -474,7 +474,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 		}
 		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;
@@ -494,7 +494,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	int modified = 0;
 	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
 	{
-		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
+		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
 		int index = newIdToIndexMap.at(*iter);
 		float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
 		this->normalize(prediction, index, sum, newIds[0]<0);

corelib/src/CMakeLists.txt

@@ -13,7 +13,9 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
+    CameraRGB.cpp
     CameraRGBD.cpp
+    CameraStereo.cpp
     CameraModel.cpp
     
     EpipolarGeometry.cpp
@@ -35,7 +37,7 @@ SET(SRC_FILES
 	util3d_surface.cpp
 	util3d_features.cpp
 	util3d_correspondences.cpp
-	util3d_conversions.cpp
+	util3d_motion_estimation.cpp
 	
 	SensorData.cpp
 	Graph.cpp

corelib/src/Camera.cpp

@@ -44,14 +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),
-	_mirroring(false),
-	_frameRateTimer(new UTimer())
+	_localTransform(localTransform),
+	_targetImageSize(0,0),
+	_frameRateTimer(new UTimer()),
+	_seq(0)
 {
 }
 
@@ -63,382 +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;
-}
-
-void Camera::setCalibration(const std::string & fileName)
-{
-	if(UFile::getExtension(fileName).compare("yaml") == 0)
+	bool warnFrameRateTooHigh = false;
+	float actualFrameRate = 0;
+	if(_imageRate>0)
 	{
-		cv::FileStorage fs;
-		fs.open(fileName, cv::FileStorage::READ);
-
-		if (!fs.isOpened())
-		{
-			UERROR("Failed to open file \"%s\"", fileName.c_str());
-			return;
-		}
-
-		cv::Mat k,d;
-
-		cv::FileNode n = fs["camera_matrix"];
-		int rows = n["rows"];
-		int cols = n["cols"];
-		std::vector<double> data;
-		n["data"] >> data;
-		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
-		{
-			k = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-		}
-
-		cv::FileNode nd = fs["distortion_coefficients"];
-		rows = nd["rows"];
-		cols = nd["cols"];
-		data.clear();
-		nd["data"] >> data;
-		if(rows > 0 && cols > 0 && (int)data.size() == rows*cols)
-		{
-			d = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-		}
-
-		if(k.empty())
-		{
-			UERROR("Failed to load \"camera_matrix\" matrix.");
-		}
-		if(d.empty())
-		{
-			UERROR("Failed to load \"distortion_coefficients\" matrix.");
-		}
-		if(!k.empty() && !d.empty())
-		{
-			this->setCalibration(k, d);
-		}
-	}
-	else
-	{
-		UERROR("Calibration file must be in \"*.yaml\" format");
-	}
-}
-
-void Camera::setCalibration(const cv::Mat & cameraMatrix, const cv::Mat & distorsionCoefficients)
-{
-	UASSERT(cameraMatrix.type() == CV_64FC1 &&
-			cameraMatrix.rows == 3 &&
-			cameraMatrix.cols == 3);
-	UASSERT(distorsionCoefficients.type() == CV_64FC1 &&
-			distorsionCoefficients.rows ==1 &&
-			(distorsionCoefficients.cols == 4 || distorsionCoefficients.cols == 5 || distorsionCoefficients.cols == 8));
-
-	_k = cameraMatrix;
-	_d = distorsionCoefficients;
-}
-
-void Camera::resetCalibration()
-{
-	_k = cv::Mat();
-	_d = cv::Mat();
-}
-
-cv::Mat Camera::takeImage()
-{
-	cv::Mat img;
-	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
-	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();
-	if(!img.empty() && !_k.empty() && !_d.empty())
+	SensorData data  = this->captureImage();
+	if(warnFrameRateTooHigh)
 	{
-		cv::Mat temp = img.clone();
-		cv::undistort(temp, img, _k, _d);
-	}
-	if(!img.empty() && _mirroring)
-	{
-		cv::flip(img,img,1);
-	}
-	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)
-	{
-		delete _dir;
-	}
-}
-
-bool CameraImages::init()
-{
-	UDEBUG("");
-	if(_dir)
-	{
-		_dir->setPath(_path, "jpg ppm png bmp pnm tiff");
+		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 tiff");
+		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);
-					}
-
-					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;
-					}
-				}
-			}
-		}
-	}
-	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

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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 {
@@ -39,13 +40,21 @@ CameraModel::CameraModel() :
 
 }
 
-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) :
+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)
+		P_(P),
+		localTransform_(localTransform)
 {
 	UASSERT(!name_.empty());
 	UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
@@ -59,7 +68,67 @@ CameraModel::CameraModel(const std::string & cameraName, const cv::Size & imageS
 	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 }
 
-bool CameraModel::load(const std::string & filePath)
+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();
@@ -68,6 +137,7 @@ bool CameraModel::load(const std::string & filePath)
 	mapX_ = cv::Mat();
 	mapY_ = cv::Mat();
 
+	std::string filePath = directory+"/"+cameraName+".yaml";
 	if(UFile::exists(filePath))
 	{
 		UINFO("Reading calibration file \"%s\"", filePath.c_str());
@@ -77,8 +147,8 @@ bool CameraModel::load(const std::string & filePath)
 		imageSize_.width = (int)fs["image_width"];
 		imageSize_.height = (int)fs["image_height"];
 		UASSERT(!name_.empty());
-		UASSERT(imageSize_.width > 0);
-		UASSERT(imageSize_.height > 0);
+		//UASSERT(imageSize_.width > 0);
+		//UASSERT(imageSize_.height > 0);
 
 		// import from ROS calibration format
 		cv::FileNode n = fs["camera_matrix"];
@@ -119,17 +189,25 @@ bool CameraModel::load(const std::string & filePath)
 
 		fs.release();
 
-		// init rectification map
-		UINFO("Initialize rectify map");
-		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+		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 & filePath)
+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());
@@ -172,6 +250,22 @@ bool CameraModel::save(const std::string & filePath)
 	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;
+}
+
 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 {
 	if(!mapX_.empty() && !mapY_.empty())
@@ -182,6 +276,7 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
 	}
 	else
 	{
+		UERROR("Cannot rectify image because the rectify map is not initialized.");
 		return raw.clone();
 	}
 }
@@ -241,11 +336,22 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 //
 //StereoCameraModel
 //
-bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName)
+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.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
+	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
 	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
 		//load rotation, translation
 		R_ = cv::Mat();
 		T_ = cv::Mat();
@@ -299,14 +405,22 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 
 			return true;
 		}
+		else
+		{
+			UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
+		}
 	}
 	return false;
 }
-bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName)
+bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
 {
-	if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
+	if(left_.save(directory) && right_.save(directory))
 	{
-		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		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());
@@ -348,7 +462,13 @@ bool StereoCameraModel::save(const std::string & directory, const std::string &
 	return false;
 }
 
-Transform StereoCameraModel::transform() const
+void StereoCameraModel::scale(double scale)
+{
+	left_.scale(scale);
+	right_.scale(scale);
+}
+
+Transform StereoCameraModel::stereoTransform() const
 {
 	if(!R_.empty() && !T_.empty())
 	{

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,8 @@ 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/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -39,21 +39,12 @@ namespace rtabmap
 // ownership transferred
 CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
-		_cameraRGBD(0),
-		_seq(0)
+		_mirroring(false),
+		_colorOnly(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 +52,6 @@ CameraThread::~CameraThread()
 	{
 		delete _camera;
 	}
-	if(_cameraRGBD)
-	{
-		delete _cameraRGBD;
-	}
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -73,75 +60,76 @@ 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())
 		{
-			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now());
-			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
+			data.setDepthOrRightRaw(cv::Mat());
 		}
-		else
+		if(_mirroring && data.cameraModels().size() == 1)
 		{
-			this->post(new CameraEvent(rgb, ++_seq, UTimer::now()));
+			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);
+			}
 		}
+
+		this->post(new CameraEvent(data, _camera->getSerial()));
 	}
 	else if(!this->isKilled())
 	{
-		if(_cameraRGBD)
-		{
-			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/DBDriver.cpp

@@ -99,6 +99,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();
@@ -390,7 +399,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-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.
@@ -406,21 +415,13 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
 	_trashesMutex.unlock();
 
 	_dbSafeAccessMutex.lock();
-	this->loadNodeDataQuery(signatures, loadMetricData);
+	this->loadNodeDataQuery(signatures);
 	_dbSafeAccessMutex.unlock();
 }
 
 void DBDriver::getNodeData(
 		int signatureId,
-		cv::Mat & imageCompressed,
-		cv::Mat & depthCompressed,
-		cv::Mat & laserScanCompressed,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform,
-		int & laserScanMaxPts) const
+		SensorData & data) const
 {
 	bool found = false;
 	// look in the trash
@@ -428,17 +429,9 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
+		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
 		{
-			imageCompressed = s->getImageCompressed();
-			depthCompressed = s->getDepthCompressed();
-			laserScanCompressed = s->getLaserScanCompressed();
-			fx = s->getFx();
-			fy = s->getFy();
-			cx = s->getCx();
-			cy = s->getCy();
-			localTransform = s->getLocalTransform();
-			laserScanMaxPts = s->getLaserScanMaxPts();
+			data = (SensorData)s->sensorData();
 			found = true;
 		}
 	}
@@ -447,31 +440,11 @@ void DBDriver::getNodeData(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
-		_dbSafeAccessMutex.unlock();
-	}
-}
-
-void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
-{
-	bool found = false;
-	// look in the trash
-	_trashesMutex.lock();
-	if(uContains(_trashSignatures, signatureId))
-	{
-		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->getImageCompressed().empty() || !s->isSaved())
-		{
-			imageCompressed = s->getImageCompressed();
-			found = true;
-		}
-	}
-	_trashesMutex.unlock();
-
-	if(!found)
-	{
-		_dbSafeAccessMutex.lock();
-		this->getNodeDataQuery(signatureId, imageCompressed);
+		std::list<Signature *> signatures;
+		Signature tmp(signatureId);
+		signatures.push_back(&tmp);
+		loadNodeDataQuery(signatures);
+		data = signatures.front()->sensorData();
 		_dbSafeAccessMutex.unlock();
 	}
 }
@@ -481,8 +454,7 @@ bool DBDriver::getNodeInfo(int signatureId,
 		int & mapId,
 		int & weight,
 		std::string & label,
-		double & stamp,
-		std::vector<unsigned char> & userData) const
+		double & stamp) const
 {
 	bool found = false;
 	// look in the trash
@@ -494,7 +466,6 @@ bool DBDriver::getNodeInfo(int signatureId,
 		weight = _trashSignatures.at(signatureId)->getWeight();
 		label = _trashSignatures.at(signatureId)->getLabel();
 		stamp = _trashSignatures.at(signatureId)->getStamp();
-		userData = _trashSignatures.at(signatureId)->getUserData();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -502,7 +473,7 @@ bool DBDriver::getNodeInfo(int signatureId,
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData);
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;
@@ -596,6 +567,33 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 	_dbSafeAccessMutex.unlock();
 }
 
+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

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;
 
@@ -70,21 +71,10 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) 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 & laserScanCompressed,
-			float & fx,
-			float & fy,
-			float & cx,
-			float & cy,
-			Transform & localTransform,
-			int & laserScanMaxPts) const;
-	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) 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;
@@ -94,6 +84,7 @@ private:
 	std::string queryStepNode() const;
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
+	std::string queryStepSensorData() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
@@ -102,25 +93,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,
-			int depth2dMaxPts) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, 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

@@ -51,7 +51,8 @@ DBReader::DBReader(const std::string & databasePath,
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
-	_currentId(_ids.end())
+	_currentId(_ids.end()),
+	_previousStamp(0)
 {
 }
 
@@ -64,7 +65,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
-	_currentId(_ids.end())
+	_currentId(_ids.end()),
+	_previousStamp(0)
 {
 }
 
@@ -148,45 +150,46 @@ void DBReader::mainLoopBegin()
 
 void DBReader::mainLoop()
 {
-	SensorData data = this->getNextData();
-	if(data.isValid())
+	OdometryEvent odom = this->getNextData();
+	if(odom.data().id())
 	{
 		int goalId = 0;
-		double previousStamp = data.stamp();
-		data.setStamp(UTimer::now());
-		if(data.userData().size() >= 6 && memcmp(data.userData().data(), "GOAL:", 5) == 0)
+		double previousStamp = odom.data().stamp();
+		odom.data().setStamp(UTimer::now());
+		if(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 = uBytes2Str(data.userData());
+			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());
-					data.setUserData(std::vector<unsigned char>());
+					odom.data().setUserData(cv::Mat());
 				}
 			}
 		}
 		if(!_odometryIgnored)
 		{
-			if(data.pose().isNull())
+			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(data));
+			this->post(new OdometryEvent(odom));
 		}
 		else
 		{
-			this->post(new CameraEvent(data));
+			this->post(new CameraEvent(odom.data()));
 		}
 
 		if(goalId > 0)
 		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
-
 			if(!_ignoreGoalDelay && _currentId != _ids.end())
 			{
 				// get stamp for the next signature to compute the delay
@@ -196,19 +199,25 @@ void DBReader::mainLoop()
 				double stamp;
 				int mapId;
 				Transform localTransform, pose;
-				std::vector<unsigned char> userData;
-				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
 				if(previousStamp && stamp && stamp > previousStamp)
 				{
 					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
 			{
 				UWARN("Goal %d detected, posting it!", goalId);
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 			}
 		}
 
@@ -237,31 +246,25 @@ void DBReader::mainLoop()
 
 }
 
-SensorData DBReader::getNextData()
+OdometryEvent DBReader::getNextData()
 {
-	SensorData data;
+	OdometryEvent odom;
 	if(_dbDriver)
 	{
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
-			cv::Mat imageBytes;
-			cv::Mat depthBytes;
-			cv::Mat laserScanBytes;
 			int mapId;
-			float fx,fy,cx,cy;
-			Transform localTransform, pose;
-			float rotVariance = 1.0f;
-			float transVariance = 1.0f;
-			std::vector<unsigned char> userData;
-			int laserScanMaxPts = 0;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+			SensorData data;
+			_dbDriver->getNodeData(*_currentId, data);
 
 			// info
+			Transform pose;
 			int weight;
 			std::string label;
 			double stamp;
-			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+			_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;
@@ -269,8 +272,7 @@ SensorData DBReader::getNextData()
 				if(links.size())
 				{
 					// assume the first is the backward neighbor, take its variance
-					rotVariance = links.begin()->second.rotVariance();
-					transVariance = links.begin()->second.transVariance();
+					infMatrix = links.begin()->second.infMatrix();
 				}
 			}
 			else
@@ -280,7 +282,7 @@ SensorData DBReader::getNextData()
 
 			int seq = *_currentId;
 			++_currentId;
-			if(imageBytes.empty())
+			if(data.imageCompressed().empty())
 			{
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
@@ -334,33 +336,16 @@ SensorData DBReader::getNextData()
 
 			if(!this->isKilled())
 			{
-				rtabmap::CompressionThread ctImage(imageBytes, true);
-				rtabmap::CompressionThread ctDepth(depthBytes, true);
-				rtabmap::CompressionThread ctLaserScan(laserScanBytes, false);
-				ctImage.start();
-				ctDepth.start();
-				ctLaserScan.start();
-				ctImage.join();
-				ctDepth.join();
-				ctLaserScan.join();
-				data = SensorData(
-						ctLaserScan.getUncompressedData(),
-						laserScanMaxPts,
-						ctImage.getUncompressedData(),
-						ctDepth.getUncompressedData(),
-						fx,fy,cx,cy,
-						localTransform,
-						pose,
-						rotVariance,
-						transVariance,
-						seq,
-						stamp,
-						userData);
-				UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
-						data.laserScan().empty()?0:1,
-						data.image().empty()?0:1,
-						data.depth().empty()?0:1,
-						data.rightImage().empty()?0:1);
+				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());
 			}
 		}
 	}
@@ -368,7 +353,7 @@ SensorData DBReader::getNextData()
 	{
 		UERROR("Not initialized...");
 	}
-	return data;
+	return odom;
 }
 
 } /* namespace rtabmap */

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>
 
-#if RTABMAP_NONFREE == 1
-#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 {
@@ -330,12 +341,29 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 }
 Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
 {
-	if(RTABMAP_NONFREE == 0 &&
-	   (type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift))
+	if(RTABMAP_NONFREE == 0)
 	{
-		UWARN("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)
 	{
@@ -432,25 +460,13 @@ 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()
 {
-#if RTABMAP_NONFREE == 1
-	if(_surf)
-	{
-		delete _surf;
-	}
-	if(_gpuSurf)
-	{
-		delete _gpuSurf;
-	}
-#endif
 }
 
 void SURF::parseParameters(const ParametersMap & parameters)
@@ -466,29 +482,30 @@ void SURF::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
 
 #if RTABMAP_NONFREE == 1
-	if(_gpuSurf)
+#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
 	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
@@ -502,10 +519,15 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
 
 #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
 	{
@@ -522,11 +544,17 @@ 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;
 #if RTABMAP_NONFREE == 1
-	if(_gpuSurf)
+	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())
@@ -557,22 +585,13 @@ 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()
 {
-#if RTABMAP_NONFREE == 1
-	if(_sift)
-	{
-		delete _sift;
-	}
-#else
-	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
-#endif
 }
 
 void SIFT::parseParameters(const ParametersMap & parameters)
@@ -586,13 +605,11 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
 
 #if RTABMAP_NONFREE == 1
-	if(_sift)
-	{
-		delete _sift;
-		_sift = 0;
-	}
-
-	_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
+#if CV_MAJOR_VERSION < 3
+	_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
+#else
+	_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
@@ -637,23 +654,13 @@ 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)
@@ -673,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
 	}
 }
 
@@ -704,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
 	{
@@ -726,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())
@@ -757,23 +794,13 @@ 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)
@@ -785,28 +812,48 @@ void FAST::parseParameters(const ParametersMap & parameters)
 	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
 	}
 }
 
@@ -815,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
 	{
@@ -832,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)
@@ -851,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;
 }
 
@@ -875,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)
@@ -898,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;
 }
 
@@ -924,18 +988,13 @@ GFTT::GFTT(const ParametersMap & parameters) :
 		_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)
@@ -949,12 +1008,11 @@ void GFTT::parseParameters(const ParametersMap & parameters)
 	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
@@ -971,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)
@@ -990,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;
 }
 
@@ -1014,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)
@@ -1037,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;
 }
 
@@ -1060,18 +1129,13 @@ 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)
@@ -1082,13 +1146,11 @@ void BRISK::parseParameters(const ParametersMap & parameters)
 	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/Graph.cpp

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Memory.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
@@ -110,17 +112,19 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para
 	return optimizer;
 }
 
-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored) :
+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) :
 		iterations_(iterations),
 		slam2d_(slam2d),
-		covarianceIgnored_(covarianceIgnored)
+		covarianceIgnored_(covarianceIgnored),
+		epsilon_(epsilon)
 {
 }
 
 Optimizer::Optimizer(const ParametersMap & parameters) :
-		iterations_(100),
-		slam2d_(false),
-		covarianceIgnored_(false)
+		iterations_(Parameters::defaultRGBDOptimizeIterations()),
+		slam2d_(Parameters::defaultRGBDOptimizeSlam2D()),
+		covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()),
+		epsilon_(Parameters::defaultRGBDOptimizeEpsilon())
 {
 	parseParameters(parameters);
 }
@@ -130,6 +134,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), iterations_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_);
 }
 
 void Optimizer::getConnectedGraph(
@@ -260,20 +265,23 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
 				AISNavigation::TreePoseGraph2::InformationMatrix inf;
 				//Identity:
-				inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
-				inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
-				inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
-				if(!isCovarianceIgnored())
+				if(isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
-						inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
-					}
+					inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
+					inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
+					inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
+				}
+				else
+				{
+					inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
+					inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
+					inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
+					inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
+					inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
+					inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
+					inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
+					inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
+					inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				int id1 = iter->first;
@@ -301,18 +309,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.rotVariance()>0)
-					{
-						inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
-						inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
-						inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
-					}
-					if(iter->second.transVariance()>0)
-					{
-						inf[3][3] = 1.0f/iter->second.transVariance(); // x
-						inf[4][4] = 1.0f/iter->second.transVariance(); // y
-						inf[5][5] = 1.0f/iter->second.transVariance(); // z
-					}
+					memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
 				}
 
 				int id1 = iter->first;
@@ -350,6 +347,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		}
 
 		UINFO("TORO iterate begin (iterations=%d)", iterations());
+		double lasterror = 0;
 		for (int i=0; i<iterations(); i++)
 		{
 			if(intermediateGraphes && i>0)
@@ -382,12 +380,14 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				}
 				intermediateGraphes->push_back(tmpPoses);
 			}
+
+			double error = 0;
 			if(isSlam2d())
 			{
 				pg2.iterate();
 
 				// compute the error and dump it
-				double error=pg2.error();
+				error=pg2.error();
 				UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
 			}
 			else
@@ -396,10 +396,19 @@ std::map<int, Transform> TOROOptimizer::optimize(
 
 				// compute the error and dump it
 				double mte, mre, are, ate;
-				double error=pg3.error(&mre, &mte, &are, &ate);
+				error=pg3.error(&mre, &mte, &are, &ate);
 				UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
 						i, pg3.getRotGain(), error, error/pg3.edges.size());
 			}
+
+			// early stop condition
+			double errorDelta = lasterror - error;
+			if(i>0 && errorDelta < this->epsilon())
+			{
+				UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+				break;
+			}
+			lasterror = error;
 		}
 		UINFO("TORO iterate end");
 
@@ -476,7 +485,7 @@ bool TOROOptimizer::saveGraph(
 		{
 			float x,y,z, yaw,pitch,roll;
 			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll, pitch, yaw);
-			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f 0 0 0 0 0 %f 0 0 0 0 %f 0 0 0 %f 0 0 %f 0 %f\n",
+			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					iter->first,
 					iter->second.to(),
 					x,
@@ -485,12 +494,27 @@ bool TOROOptimizer::saveGraph(
 					roll,
 					pitch,
 					yaw,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.rotVariance()>0?1.0f/iter->second.rotVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f,
-					iter->second.transVariance()>0?1.0f/iter->second.transVariance():1.0f);
+					iter->second.infMatrix().at<double>(0,0),
+					iter->second.infMatrix().at<double>(0,1),
+					iter->second.infMatrix().at<double>(0,2),
+					iter->second.infMatrix().at<double>(0,3),
+					iter->second.infMatrix().at<double>(0,4),
+					iter->second.infMatrix().at<double>(0,5),
+					iter->second.infMatrix().at<double>(1,1),
+					iter->second.infMatrix().at<double>(1,2),
+					iter->second.infMatrix().at<double>(1,3),
+					iter->second.infMatrix().at<double>(1,4),
+					iter->second.infMatrix().at<double>(1,5),
+					iter->second.infMatrix().at<double>(2,2),
+					iter->second.infMatrix().at<double>(2,3),
+					iter->second.infMatrix().at<double>(2,4),
+					iter->second.infMatrix().at<double>(2,5),
+					iter->second.infMatrix().at<double>(3,3),
+					iter->second.infMatrix().at<double>(3,4),
+					iter->second.infMatrix().at<double>(3,5),
+					iter->second.infMatrix().at<double>(4,4),
+					iter->second.infMatrix().at<double>(4,5),
+					iter->second.infMatrix().at<double>(5,5));
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
 		fclose(file);
@@ -674,15 +698,15 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						information(2,2) = 1.0f/iter->second.rotVariance(); // theta
-					}
+					information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+					information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+					information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+					information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+					information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+					information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+					information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+					information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+					information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}
 
 				g2o::EdgeSE2 * e = new g2o::EdgeSE2();
@@ -701,18 +725,7 @@ std::map<int, Transform> G2OOptimizer::optimize(
 				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
 				if(!isCovarianceIgnored())
 				{
-					if(iter->second.transVariance()>0)
-					{
-						information(0,0) = 1.0f/iter->second.transVariance(); // x
-						information(1,1) = 1.0f/iter->second.transVariance(); // y
-						information(2,2) = 1.0f/iter->second.transVariance(); // z
-					}
-					if(iter->second.rotVariance()>0)
-					{
-						information(3,3) = 1.0f/iter->second.rotVariance(); // roll
-						information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
-						information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
-					}
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
 				}
 
 				Eigen::Affine3d a = iter->second.transform().toEigen3d();
@@ -934,6 +947,61 @@ std::multimap<int, int>::iterator findLink(
 	}
 	return links.end();
 }
+std::multimap<int, Link>::const_iterator findLink(
+		const std::multimap<int, Link> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, Link>::const_iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second.to() == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second.to() == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
+
+std::multimap<int, int>::const_iterator findLink(
+		const std::multimap<int, int> & links,
+		int from,
+		int to)
+{
+	std::multimap<int, int>::const_iterator iter = links.find(from);
+	while(iter != links.end() && iter->first == from)
+	{
+		if(iter->second == to)
+		{
+			return iter;
+		}
+		++iter;
+	}
+
+	// let's try to -> from
+	iter = links.find(to);
+	while(iter != links.end() && iter->first == to)
+	{
+		if(iter->second == from)
+		{
+			return iter;
+		}
+		++iter;
+	}
+	return links.end();
+}
 
 std::map<int, Transform> radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
@@ -1247,6 +1315,127 @@ std::list<std::pair<int, Transform> > computePath(
 	return path;
 }
 
+
+// return path starting from "fromId" (Identity pose for the first node)
+std::list<std::pair<int, Transform> > computePath(
+		int fromId,
+		int toId,
+		const Memory * memory,
+		bool lookInDatabase,
+		bool updateNewCosts)
+{
+	UASSERT(memory!=0);
+	UASSERT(fromId>=0);
+	UASSERT(toId>=0);
+	std::list<std::pair<int, Transform> > path;
+
+	std::multimap<int, Link> allLinks;
+	if(lookInDatabase)
+	{
+		// Faster to load all links in one query
+		UTimer t;
+		allLinks = memory->getAllLinks(lookInDatabase);
+		UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
+	}
+
+	//dijkstra
+	int startNode = fromId;
+	int endNode = toId;
+	std::map<int, Node> nodes;
+	nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity())));
+	std::priority_queue<Pair, std::vector<Pair>, Order> pq;
+	std::multimap<float, int> pqmap;
+	if(updateNewCosts)
+	{
+		pqmap.insert(std::make_pair(0, startNode));
+	}
+	else
+	{
+		pq.push(Pair(startNode, 0));
+	}
+
+	while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
+	{
+		Node * currentNode;
+		if(updateNewCosts)
+		{
+			currentNode = &nodes.find(pqmap.begin()->second)->second;
+			pqmap.erase(pqmap.begin());
+		}
+		else
+		{
+			currentNode = &nodes.find(pq.top().first)->second;
+			pq.pop();
+		}
+
+		currentNode->setClosed(true);
+
+		if(currentNode->id() == endNode)
+		{
+			while(currentNode->id()!=startNode)
+			{
+				path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
+				currentNode = &nodes.find(currentNode->fromId())->second;
+			}
+			path.push_front(std::make_pair(startNode, currentNode->pose()));
+			break;
+		}
+
+		// lookup neighbors
+		std::map<int, Link> links;
+		if(allLinks.size() == 0)
+		{
+			links = memory->getLinks(currentNode->id(), lookInDatabase);
+		}
+		else
+		{
+			for(std::multimap<int, Link>::const_iterator iter = allLinks.lower_bound(currentNode->id());
+				iter!=allLinks.end() && iter->first == currentNode->id();
+				++iter)
+			{
+				links.insert(std::make_pair(iter->second.to(), iter->second));
+			}
+		}
+		for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+		{
+			std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
+			if(nodeIter == nodes.end())
+			{
+				Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform());
+				n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm());
+				nodes.insert(std::make_pair(iter->second.to(), n));
+				if(updateNewCosts)
+				{
+					pqmap.insert(std::make_pair(n.totalCost(), n.id()));
+				}
+				else
+				{
+					pq.push(Pair(n.id(), n.totalCost()));
+				}
+			}
+			else if(updateNewCosts && nodeIter->second.isOpened())
+			{
+				float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
+				if(nodeIter->second.costSoFar() > newCostSoFar)
+				{
+					// update the cost in the priority queue
+					for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
+					{
+						if(mapIter->second == nodeIter->first)
+						{
+							pqmap.erase(mapIter);
+							nodeIter->second.setCostSoFar(newCostSoFar);
+							pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
+							break;
+						}
+					}
+				}
+			}
+		}
+	}
+	return path;
+}
+
 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose)

corelib/src/Odometry.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "ParticleFilter.h"
 
 namespace rtabmap {
 
@@ -41,11 +43,21 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultOdomForce2D()),
+		_holonomic(Parameters::defaultOdomHolonomic()),
+		_particleFiltering(Parameters::defaultOdomParticleFiltering()),
+		_particleSize(Parameters::defaultOdomParticleSize()),
+		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
+		_particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
+		_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
+		_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
-		_pnpEstimation(Parameters::defaultOdomPnPEstimation()),
+		_estimationType(Parameters::defaultOdomEstimationType()),
 		_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
 		_pnpFlags(Parameters::defaultOdomPnPFlags()),
-		_resetCurrentCount(0)
+		_resetCurrentCount(0),
+		previousStamp_(0),
+		previousTransform_(Transform::getIdentity()),
+		distanceTravelled_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
@@ -55,26 +67,86 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
+	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
-	Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation);
+	Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType);
 	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
 	Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
+	Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
+	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
+	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
+	Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
+	Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR);
+	Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR);
+	UASSERT(_particleNoiseT>0);
+	UASSERT(_particleLambdaT>0);
+	UASSERT(_particleNoiseR>0);
+	UASSERT(_particleLambdaR>0);
+	if(_particleFiltering)
+	{
+		filters_.resize(6);
+		for(unsigned int i = 0; i<filters_.size(); ++i)
+		{
+			if(i<3)
+			{
+				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
+			}
+			else
+			{
+				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
+			}
+		}
+	}
+}
+
+Odometry::~Odometry()
+{
+	for(unsigned int i=0; i<filters_.size(); ++i)
+	{
+		delete filters_[i];
+	}
+	filters_.clear();
 }
 
 void Odometry::reset(const Transform & initialPose)
 {
+	previousTransform_.setIdentity();
 	_resetCurrentCount = 0;
-	if(_force2D)
+	previousStamp_ = 0;
+	distanceTravelled_ = 0;
+	if(_force2D || filters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
 		initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-		if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
+
+		if(_force2D)
 		{
-			UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
+			if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
+			{
+				UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
+			}
+			z = 0;
+			roll = 0;
+			yaw = 0;
+			Transform pose(x, y, z, roll, pitch, yaw);
+			_pose = pose;
+		}
+		else
+		{
+			_pose = initialPose;
+		}
+
+		if(filters_.size())
+		{
+			UASSERT(filters_.size() == 6);
+			filters_[0]->init(x);
+			filters_[1]->init(y);
+			filters_[2]->init(z);
+			filters_[3]->init(roll);
+			filters_[4]->init(pitch);
+			filters_[5]->init(yaw);
 		}
-		Transform pose(x, y, 0, 0, 0, yaw);
-		_pose = pose;
 	}
 	else
 	{
@@ -89,16 +161,12 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		_pose.setIdentity(); // initialized
 	}
 
-	UASSERT(!data.image().empty());
-	if(dynamic_cast<OdometryMono*>(this) == 0)
-	{
-		UASSERT(!data.depthOrRightImage().empty());
-	}
+	UASSERT(!data.imageRaw().empty());
 
-	if(data.fx() <= 0 || data.fyOrBaseline() <= 0)
+	if(!data.stereoCameraModel().isValid() &&
+	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValid()))
 	{
-		UERROR("Rectified images required! Calibrate your camera. (fx=%f, fy/baseline=%f, cx=%f, cy=%f)",
-				data.fx(), data.fyOrBaseline(), data.cx(), data.cy());
+		UERROR("Rectified images required! Calibrate your camera.");
 		return Transform();
 	}
 
@@ -107,19 +175,100 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 
 	if(info)
 	{
-		info->time = time.elapsed();
+		info->timeEstimation = time.ticks();
 		info->lost = t.isNull();
+		info->stamp = data.stamp();
+		info->interval = data.stamp() - previousStamp_;
+		info->transform = t;
 	}
 
+	previousTransform_.setIdentity();
+	previousStamp_ = data.stamp();
+
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		if(_force2D)
+		if(_force2D || !_holonomic || filters_.size())
 		{
 			float x,y,z, roll,pitch,yaw;
 			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-			t = Transform(x,y,0, 0,0,yaw);
+
+			if(filters_.size())
+			{
+				UASSERT(filters_.size()==6);
+				if(_pose.isIdentity())
+				{
+					filters_[0]->init(x);
+					filters_[1]->init(y);
+					filters_[2]->init(z);
+					filters_[3]->init(roll);
+					filters_[4]->init(pitch);
+					filters_[5]->init(yaw);
+				}
+				else
+				{
+					x = filters_[0]->filter(x);
+					y = filters_[1]->filter(y);
+					yaw = filters_[5]->filter(yaw);
+
+					if(!_holonomic)
+					{
+						// arc trajectory around ICR
+						float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
+						if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
+						{
+							y = tmpY;
+						}
+						else
+						{
+							yaw = (atan(x/y)*2.0f-CV_PI)*-1;
+						}
+					}
+
+					if(!_force2D)
+					{
+						z = filters_[2]->filter(z);
+						roll = filters_[3]->filter(roll);
+						pitch = filters_[4]->filter(pitch);
+					}
+				}
+
+				if(info)
+				{
+					info->timeParticleFiltering = time.ticks();
+				}
+			}
+			else if(!_holonomic)
+			{
+				// arc trajectory around ICR
+				float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
+				if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
+				{
+					y = tmpY;
+				}
+				else
+				{
+					yaw = (atan(x/y)*2.0f-CV_PI)*-1;
+				}
+			}
+			UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
+					uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
+					uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
+							x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
+			t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
+
+			if(info && filters_.size())
+			{
+				info->transformFiltered = t;
+			}
+		}
+
+		previousTransform_ = t;
+		if(info)
+		{
+			distanceTravelled_ += t.getNorm();
+			info->distanceTravelled = distanceTravelled_;
 		}
 
 		return _pose *= t; // updated

corelib/src/OdometryBOW.cpp

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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"
@@ -49,9 +51,12 @@ 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())));
@@ -101,10 +106,71 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 		}
 	}
 
-	_memory = new Memory(customParameters);
-	if(!_memory->init("", false, ParametersMap()))
+	if(_fixedLocalMapPath.empty())
 	{
-		UERROR("Error initializing the memory for BOW Odometry.");
+		_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"));
+		_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());
+		}
 	}
 }
 
@@ -117,9 +183,16 @@ OdometryBOW::~OdometryBOW()
 
 void OdometryBOW::reset(const Transform & initialPose)
 {
-	Odometry::reset(initialPose);
-	_memory->init("", false, ParametersMap());
-	localMap_.clear();
+	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
@@ -136,11 +209,10 @@ Transform OdometryBOW::computeTransform(
 	}
 
 	double variance = 0;
-	int inliers = 0;
+	int inliersCount = 0;
 	int correspondences = 0;
 	int nFeatures = 0;
 
-	const Signature * previousSignature = _memory->getLastWorkingSignature();
 	if(_memory->update(data))
 	{
 		const Signature * newSignature = _memory->getLastWorkingSignature();
@@ -153,118 +225,39 @@ Transform OdometryBOW::computeTransform(
 			}
 		}
 
-		if(previousSignature && newSignature)
+		if(localMap_.size() && newSignature)
 		{
 			Transform transform;
 			if((int)localMap_.size() >= this->getMinInliers())
 			{
-				if(this->isPnPEstimationUsed())
+				std::vector<int> matches, inliers;
+				Transform t;
+				if(this->getEstimationType() == 1) // PnP
 				{
-					if((int)newSignature->getWords().size() >= this->getMinInliers())
+					// 3D to 2D
+					if(data.cameraModels().size() > 1)
 					{
-						// find correspondences
-						std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
-						std::vector<cv::Point3f> objectPoints(ids.size());
-						std::vector<cv::Point2f> imagePoints(ids.size());
-						int oi=0;
-						std::vector<int> matches(ids.size());
-						for(unsigned int i=0; i<ids.size(); ++i)
-						{
-							if(localMap_.count(ids[i]) == 1)
-							{
-								pcl::PointXYZ pt = localMap_.find(ids[i])->second;
-								objectPoints[oi].x = pt.x;
-								objectPoints[oi].y = pt.y;
-								objectPoints[oi].z = pt.z;
-								imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
-								matches[oi++] = ids[i];
-							}
-						}
+						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];
 
-						objectPoints.resize(oi);
-						imagePoints.resize(oi);
-						matches.resize(oi);
-
-						if(this->isInfoDataFilled() && info)
-						{
-							info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
-						}
-						correspondences = (int)matches.size();
-
-						if((int)matches.size() >= this->getMinInliers())
-						{
-							//PnPRansac
-							cv::Mat K = (cv::Mat_<double>(3,3) <<
-								data.fx(), 0, data.cx(),
-								0, data.fy()>0?data.fy():data.fx(), data.cy(),
-								0, 0, 1);
-							Transform guess = (this->getPose() * data.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<int> inliersV;
-							cv::solvePnPRansac(objectPoints,
-									imagePoints,
-									K,
-									cv::Mat(),
-									rvec,
-									tvec,
-									true,
-									this->getIterations(),
-									this->getPnPReprojError(),
-									0,
-									inliersV,
-									this->getPnPFlags());
-
-							inliers = (int)inliersV.size();
-							if((int)inliersV.size() >= this->getMinInliers())
-							{
-								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));
-
-								// make it incremental
-								transform = (data.localTransform() * pnp * this->getPose()).inverse();
-
-								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
-
-								// compute variance (like in PCL computeVariance() method of sac_model.h)
-								std::vector<float> errorSqrdDists(inliersV.size());
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
-									UASSERT(iter != newSignature->getWords3().end());
-									const cv::Point3f & objPt = objectPoints[inliersV[i]];
-									pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
-									errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-								}
-								std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-								double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-								variance = 2.1981 * median_error_sqr;
-							}
-							else
-							{
-								UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
-							}
-
-							if(this->isInfoDataFilled() && info && inliersV.size())
-							{
-								info->wordInliers.resize(inliersV.size());
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									info->wordInliers[i] = matches[inliersV[i]];
-								}
-							}
-						}
-						else
-						{
-							UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
-						}
+						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
 					{
@@ -273,76 +266,51 @@ Transform OdometryBOW::computeTransform(
 				}
 				else
 				{
+					// 3D to 3D
 					if((int)newSignature->getWords3().size() >= this->getMinInliers())
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
-						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
-
-						// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
-						// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
-						// by depth here is wrong!
-						std::set<int> uniqueCorrespondences;
-						util3d::findCorrespondences(
+						t = util3d::estimateMotion3DTo3D(
 								localMap_,
-								newSignature->getWords3(),
-								*inliers1,
-								*inliers2,
-								0,
-								&uniqueCorrespondences);
-
-						UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
-
-						if(this->isInfoDataFilled() && info)
-						{
-							info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
-						}
-
-						correspondences = (int)inliers1->size();
-						if((int)inliers1->size() >= this->getMinInliers())
-						{
-							// the transform returned is global odometry pose, not incremental one
-							std::vector<int> inliersV;
-							Transform t = util3d::transformFromXYZCorrespondences(
-									inliers2,
-									inliers1,
-									this->getInlierDistance(),
-									this->getIterations(),
-									this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-									&inliersV,
-									&variance);
-
-							inliers = (int)inliersV.size();
-							if(!t.isNull() && inliers >= this->getMinInliers())
-							{
-								// make it incremental
-								transform = this->getPose().inverse() * t;
-
-								UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
-							}
-							else
-							{
-								UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
-							}
-
-							if(this->isInfoDataFilled() && info && inliersV.size())
-							{
-								info->wordInliers.resize(inliersV.size());
-								for(unsigned int i=0; i<inliersV.size(); ++i)
-								{
-									info->wordInliers[i] = info->wordMatches[inliersV[i]];
-								}
-							}
-						}
-						else
-						{
-							UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
-						}
+								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
 			{
@@ -353,9 +321,10 @@ Transform OdometryBOW::computeTransform(
 			{
 				_memory->deleteLocation(newSignature->id());
 			}
-			else
+			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)
 				{
@@ -399,14 +368,18 @@ Transform OdometryBOW::computeTransform(
 					}
 				}
 			}
+			else
+			{
+				// fixed local map, just delete the new signature
+				output = transform;
+				_memory->deleteLocation(newSignature->id());
+			}
 		}
-		else if(!previousSignature && newSignature)
+		else if(newSignature)
 		{
-			localMap_.clear();
-
 			int count = 0;
 			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-			if((int)uniques.size() >= this->getMinInliers())
+			if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers())
 			{
 				output.setIdentity();
 
@@ -443,7 +416,7 @@ Transform OdometryBOW::computeTransform(
 	if(info)
 	{
 		info->variance = variance;
-		info->inliers = inliers;
+		info->inliers = inliersCount;
 		info->matches = correspondences;
 		info->features = nFeatures;
 		info->localMapSize = (int)localMap_.size();
@@ -453,7 +426,7 @@ Transform OdometryBOW::computeTransform(
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			nFeatures,
-			inliers,
+			inliersCount,
 			correspondences,
 			variance,
 			(int)localMap_.size(),

corelib/src/OdometryICP.cpp

@@ -72,25 +72,32 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	bool hasConverged = false;
 	double variance = 0;
 	unsigned int minPoints = 100;
-	if(!data.depth().empty())
+	if(!data.depthOrRightRaw().empty())
 	{
-		if(data.depth().type() == CV_8UC1)
+		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.depth(),
-						data.fx(),
-						data.fy(),
-						data.cx(),
-						data.cy(),
+						data.depthOrRightRaw(),
+						cameraModel.fx(),
+						cameraModel.fy(),
+						cameraModel.cx(),
+						cameraModel.cy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
 						_samples,
-						data.localTransform());
+						cameraModel.localTransform());
 
 		if(_pointToPlane)
 		{
@@ -105,14 +112,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 
 			if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
 			{
-				int correspondences = 0;
-				Transform transform = util3d::icpPointToPlane(newCloud,
+				pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
+				Transform transform = util3d::icpPointToPlane(
+						newCloud,
 						_previousCloudNormal,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						&hasConverged,
-						&variance,
-						&correspondences);
+						hasConverged,
+						*newCloudRegistered);
+
+				int correspondences = 0;
+				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());
@@ -140,14 +155,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 			//point to point
 			if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
 			{
-				int correspondences = 0;
-				Transform transform = util3d::icp(newCloudXYZ,
+				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
+				Transform transform = util3d::icp(
+						newCloudXYZ,
 						_previousCloud,
 						_maxCorrespondenceDistance,
 						_maxIterations,
-						&hasConverged,
-						&variance,
-						&correspondences);
+						hasConverged,
+						*newCloudRegistered);
+
+				int correspondences = 0;
+				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());

corelib/src/OdometryMono.cpp

@@ -50,6 +50,11 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	flowIterations_(Parameters::defaultOdomFlowIterations()),
 	flowEps_(Parameters::defaultOdomFlowEps()),
 	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	stereoWinSize_(Parameters::defaultStereoWinSize()),
+	stereoIterations_(Parameters::defaultStereoIterations()),
+	stereoEps_(Parameters::defaultStereoEps()),
+	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
+	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
 	localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
 	initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
 	initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -64,6 +69,12 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
 	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
 
+	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::kOdomMonoInitMinFlow(), initMinFlow_);
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
 	Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
@@ -139,7 +150,7 @@ void OdometryMono::reset(const Transform & initialPose)
 	Odometry::reset(initialPose);
 	memory_->init("", false, ParametersMap());
 	localMap_.clear();
-	refDepth_ = cv::Mat();
+	refDepthOrRight_ = cv::Mat();
 	cornersMap_.clear();
 	keyFrameWords3D_.clear();
 	keyFramePoses_.clear();
@@ -147,11 +158,24 @@ void OdometryMono::reset(const Transform & initialPose)
 
 Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
 {
-	UASSERT(!data.image().empty());
-	UASSERT(data.fx());
+	Transform output;
+
+	if(data.imageRaw().empty())
+	{
+		UERROR("Image empty! Cannot compute odometry...");
+		return output;
+	}
+
+	if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid())))
+	{
+		UERROR("Odometry cannot be done without calibration or on multi-camera!");
+		return output;
+	}
+
+
+	const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
 
 	UTimer timer;
-	Transform output;
 
 	int inliers = 0;
 	int correspondences = 0;
@@ -159,13 +183,13 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 	cv::Mat newFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newFrame = data.image().clone();
+		newFrame = data.imageRaw().clone();
 	}
 
 	if(memory_->getStMem().size() >= 1)
@@ -190,11 +214,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 				nFeatures = (int)newS->getWords().size();
 				if((int)newS->getWords().size() > this->getMinInliers())
 				{
-					cv::Mat K = (cv::Mat_<double>(3,3) <<
-						data.fx(), 0, data.cx(),
-						0, data.fy()==0?data.fx():data.fy(), data.cy(),
-						0, 0, 1);
-					Transform guess = (this->getPose() * data.localTransform()).inverse();
+					cv::Mat K = cameraModel.K();
+					Transform guess = (this->getPose() * cameraModel.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(),
@@ -216,7 +237,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					UDEBUG("project points to previous image");
 					std::vector<cv::Point2f> prevImagePoints;
 					const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
-					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
+					Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
 					cv::Mat prevR = (cv::Mat_<double>(3,3) <<
 							(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
 							(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
@@ -240,8 +261,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					{
 						if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
 						   uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
-						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
-						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
+						   uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
+						   uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
 						{
 							refCorners[oi] = prevImagePoints[i];
 							newCorners[oi] = imagePoints[i];
@@ -273,7 +294,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 					std::vector<float> err;
 					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 					cv::calcOpticalFlowPyrLK(
-							prevS->getImageRaw(),
+							prevS->sensorData().imageRaw(),
 							newFrame,
 							refCorners,
 							newCorners,
@@ -339,7 +360,11 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								true,
 								this->getIterations(),
 								this->getPnPReprojError(),
-								0,
+#if CV_MAJOR_VERSION < 3
+								0, // min inliers
+#else
+								0.99, // confidence
+#endif
 								inliersV,
 								this->getPnPFlags());
 
@@ -357,7 +382,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							Transform pnp = Transform(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));
-							output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
+							output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
 
 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
@@ -391,7 +416,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							UDEBUG("cameraTransform guess=  %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
 							if(cameraTransform.getNorm() < minTranslation_)
 							{
-								UWARN("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
+								UINFO("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
 										cameraTransform.getNorm(), minTranslation_);
 							}
 							else
@@ -402,9 +427,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
 										previousS->getWords(),
 										newS->getWords(),
-										data.fx(), data.fy()?data.fy():data.fx(),
-										data.cx(), data.cy(),
-										data.localTransform(),
+										cameraModel,
 										cameraTransform,
 										this->getIterations(),
 										this->getPnPReprojError(),
@@ -515,7 +538,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 			std::vector<float> err;
 			UDEBUG("cv::calcOpticalFlowPyrLK() begin");
 			cv::calcOpticalFlowPyrLK(
-					refS->getImageRaw(),
+					refS->sensorData().imageRaw(),
 					newFrame,
 					refCorners,
 					refCornersGuess,
@@ -599,7 +622,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 						cv::RANSAC,
 						fundMatrixReprojError_,
 						fundMatrixConfidence_);
-				std::cout << "F=" << F << std::endl;
+				//std::cout << "F=" << F << std::endl;
 
 				if(!F.empty())
 				{
@@ -652,10 +675,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 						//UDEBUG("Correcting matches...done!");
 
 						UDEBUG("Computing P...");
-						cv::Mat K = (cv::Mat_<double>(3,3) <<
-									data.fx(), 0, data.cx(),
-									0, data.fy()==0?data.fx():data.fy(), data.cy(),
-									0, 0, 1);
+						cv::Mat K = cameraModel.K();
 
 						cv::Mat Kinv = K.inv();
 						cv::Mat E = K.t()*F*K;
@@ -688,7 +708,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							P0.at<double>(2,2) = 1;
 
 							UDEBUG("Computing P...done!");
-							std::cout << "P=" << P << std::endl;
+							//std::cout << "P=" << P << std::endl;
 
 							cv::Mat R, T;
 							EpipolarGeometry::findRTFromP(P, R, T);
@@ -707,6 +727,44 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 
 							oi = 0;
 							UASSERT(newCorners.size() == cloud->size());
+
+							pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
+
+							if(!refDepthOrRight_.empty())
+							{
+								if(refDepthOrRight_.type() == CV_8UC1)
+								{
+									 newCorners3D = util3d::generateKeypoints3DStereo(
+											 refCorners,
+											 refS->sensorData().imageRaw(),
+											 refDepthOrRight_,
+											 cameraModel.fx(),
+											 data.stereoCameraModel().baseline(),
+											 cameraModel.cx(),
+											 cameraModel.cy(),
+											 Transform::getIdentity(),
+											 stereoWinSize_,
+											 stereoMaxLevel_,
+											 stereoIterations_,
+											 stereoEps_,
+											 stereoMaxSlope_ );
+								}
+								else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
+								{
+									std::vector<cv::KeyPoint> tmpKpts;
+									cv::KeyPoint::convert(refCorners, tmpKpts);
+									CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy());
+									 newCorners3D = util3d::generateKeypoints3DDepth(
+											 tmpKpts,
+											 refDepthOrRight_,
+											 m);
+								}
+								else
+								{
+									UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type());
+								}
+							}
+
 							for(unsigned int i=0; i<cloud->size(); ++i)
 							{
 								if(cloud->at(i).z>0)
@@ -714,9 +772,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									imagePoints[oi] = newCorners[i];
 									tmpCornersId[oi] = cornerIds[i];
 									(*inliersRef)[oi] = cloud->at(i);
-									if(!refDepth_.empty())
+									if(!newCorners3D->empty())
 									{
-										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
+										(*inliersRefGuess)[oi] = newCorners3D->at(i);
 									}
 									++oi;
 								}
@@ -732,7 +790,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							//estimate scale
 							float scale = 1;
 							std::multimap<float, float> scales; // <variance, scale>
-							if(!refDepth_.empty()) // scale known
+							if(!newCorners3D->empty()) // scale known
 							{
 								UASSERT(inliersRefGuess->size() == inliersRef->size());
 								for(unsigned int i=0; i<inliersRef->size(); ++i)
@@ -741,6 +799,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 									{
 										float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
 										std::vector<float> errorSqrdDists(inliersRef->size());
+										oi = 0;
 										for(unsigned int j=0; j<inliersRef->size(); ++j)
 										{
 											if(cloud->at(j).z>0)
@@ -750,30 +809,39 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 												refPt.y *= s;
 												refPt.z *= s;
 												const pcl::PointXYZ & guess = inliersRefGuess->at(j);
-												errorSqrdDists[j] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
+												errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
 											}
 										}
-										std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-										double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-										float variance = 2.1981 * median_error_sqr;
-										//UDEBUG("scale %d = %f variance = %f", i, s, variance);
-										if(variance > 0)
+										errorSqrdDists.resize(oi);
+										if(errorSqrdDists.size() > 2)
 										{
-											scales.insert(std::make_pair(variance, s));
+											std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+											double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+											float variance = 2.1981 * median_error_sqr;
+											//UDEBUG("scale %d = %f variance = %f", i, s, variance);
+											if(variance > 0)
+											{
+												scales.insert(std::make_pair(variance, s));
+											}
 										}
 									}
 								}
-								UASSERT(scales.size());
-
-								scale = scales.begin()->second;
-								UDEBUG("scale used = %f (variance=%f)", scale, scales.begin()->first);
-
-								maxVariance_ = 0.01;
-								UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
-								if(scales.begin()->first > 0.01)
+								if(scales.size() == 0)
 								{
-									UWARN("Too high variance %f (should be < 0.01)");
-									reject = true; // 20 cm for good initialization
+									UWARN("No scales found!?");
+									reject = true;
+								}
+								else
+								{
+									scale = scales.begin()->second;
+									UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size());
+
+									UDEBUG("Max noise variance = %f current variance=%f", maxVariance_, scales.begin()->first);
+									if(scales.begin()->first > maxVariance_)
+									{
+										UWARN("Too high variance %f (should be < %f)", scales.begin()->first, maxVariance_);
+										reject = true; // 20 cm for good initialization
+									}
 								}
 
 							}
@@ -783,7 +851,6 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								Eigen::Vector4f centroid;
 								pcl::compute3DCentroid(*inliersRef, centroid);
 								scale = 1.0f / centroid[2];
-								maxVariance_ = 0.01;
 							}
 							else
 							{
@@ -813,7 +880,11 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										false,
 										this->getIterations(),
 										this->getPnPReprojError(),
-										0,
+#if CV_MAJOR_VERSION < 3
+										0, // min inliers
+#else
+										0.99, // confidence
+#endif
 										inliersPnP,
 										this->getPnPFlags());
 
@@ -824,7 +895,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 											  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));
 
-								output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
+								output = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse();
 								if(output.getNorm() < minTranslation_*5)
 								{
 									reject = true;
@@ -844,7 +915,9 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										int index  =inliersPnP.at(i);
 										int id = cornerIds[index];
 										UASSERT(id > 0 && id <= *wordsId.rbegin());
-										pcl::PointXYZ pt =  util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
+										pcl::PointXYZ pt =  util3d::transformPoint(
+												pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z),
+												this->getPose()*cameraModel.localTransform());
 										localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
 										keyFrameWords3D.insert(std::make_pair(id, pt));
 									}
@@ -888,9 +961,12 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 			{
 				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 				{
-					cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
+					if(words.count(iter->first) == 1)
+					{
+						cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
+					}
 				}
-				refDepth_ = data.depth().clone();
+				refDepthOrRight_ = data.depthOrRightRaw().clone();
 				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
 			}
 			else

corelib/src/OdometryOpticalFlow.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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"
@@ -121,57 +122,81 @@ Transform OdometryOpticalFlow::computeTransform(
 		const SensorData & data,
 		OdometryInfo * info)
 {
-	UDEBUG("");
+	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;
 	}
 
-	if(!data.rightImage().empty())
-	{
-		//stereo
-		return computeTransformStereo(data, info);
-	}
-	else
-	{
-		//rgbd
-		return computeTransformRGBD(data, info);
-	}
-}
-
-Transform OdometryOpticalFlow::computeTransformStereo(
-		const SensorData & data,
-		OdometryInfo * info)
-{
-	UTimer timer;
-	Transform output;
-
-	double variance = 0;
-	int inliers = 0;
-	int correspondences = 0;
-
 	cv::Mat newLeftFrame;
 	// convert to grayscale
-	if(data.image().channels() > 1)
+	if(data.imageRaw().channels() > 1)
 	{
-		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+		cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
 	}
 	else
 	{
-		newLeftFrame = data.image().clone();
+		newLeftFrame = data.imageRaw().clone();
 	}
-	cv::Mat newRightFrame = data.rightImage().clone();
 
 	std::vector<cv::Point2f> newCorners;
-	UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
-	if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size())
+	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())
 	{
-		UDEBUG("");
+		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,
@@ -179,155 +204,54 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 				newCorners,
 				status,
 				err,
-				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+				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, 1e-4);
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
 		UDEBUG("cv::calcOpticalFlowPyrLK() end");
 
-		std::vector<cv::Point2f> lastCornersKept(status.size());
+		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])
 			{
-				lastCornersKept[ki] = refCorners_[i];
+				refCorners3DKept->at(ki) = refCorners3D_->at(i);
+				objectPointsKept[ki] = objectPoints[i];
+				refCornersKept[ki] = refCorners_[i];
 				newCornersKept[ki] = newCorners[i];
 				++ki;
 			}
 		}
-		lastCornersKept.resize(ki);
+		refCorners3DKept->resize(ki);
+		objectPointsKept.resize(ki);
+		refCornersKept.resize(ki);
 		newCornersKept.resize(ki);
 
 		if(ki && ki >= this->getMinInliers())
 		{
-			std::vector<unsigned char> statusLast;
-			std::vector<float> errLast;
-			std::vector<cv::Point2f> lastCornersKeptRight;
-			UDEBUG("previous stereo disparity");
-			cv::calcOpticalFlowPyrLK(
-						refFrame_,
-						refRightFrame_,
-						lastCornersKept,
-						lastCornersKeptRight,
-						statusLast,
-						errLast,
-						cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
-						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
-						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
-
-			UDEBUG("new stereo disparity");
-			std::vector<unsigned char> statusNew;
-			std::vector<float> errNew;
-			std::vector<cv::Point2f> newCornersKeptRight;
-			cv::calcOpticalFlowPyrLK(
-						newLeftFrame,
-						newRightFrame,
-						newCornersKept,
-						newCornersKeptRight,
-						statusNew,
-						errNew,
-						cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
-						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
-						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
-
-			if(this->isPnPEstimationUsed())
+			if(this->getEstimationType() == 1) // PnP
 			{
 				// find correspondences
 				if(this->isInfoDataFilled() && info)
 				{
-					info->refCorners.resize(statusLast.size());
-					info->newCorners.resize(statusLast.size());
+					info->refCorners = refCornersKept;
+					info->newCorners = newCornersKept;
 				}
 
-				int flowInliers = 0;
-				std::vector<cv::Point3f> objectPoints(statusLast.size());
-				std::vector<cv::Point2f> imagePoints(statusLast.size());
-				std::vector<pcl::PointXYZ> image3DPoints(statusLast.size());
-				int oi=0;
-				float bad_point = std::numeric_limits<float>::quiet_NaN ();
-				for(unsigned int i=0; i<statusLast.size(); ++i)
-				{
-					if(statusLast[i])
-					{
-						float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
-						float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
-						float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
-						float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
-						if(lastDisparity > 0.0f && lastSlope < stereoMaxSlope_)
-						{
-							pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
-									lastCornersKept[i],
-									lastDisparity,
-									data.cx(), data.cy(), data.fx(), data.baseline());
-
-							if(pcl::isFinite(lastPt3D) &&
-							   (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
-							{
-								//Add 3D correspondences!
-								lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
-								objectPoints[oi].x = lastPt3D.x;
-								objectPoints[oi].y = lastPt3D.y;
-								objectPoints[oi].z = lastPt3D.z;
-								imagePoints[oi] = newCornersKept.at(i);
-
-								// new 3D points, used to compute variance
-								image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
-								if(newDisparity > 0.0f && newSlope < stereoMaxSlope_)
-								{
-									pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
-											newCornersKept[i],
-											newDisparity,
-											data.cx(), data.cy(), data.fx(), data.baseline());
-									if(pcl::isFinite(newPt3D) &&
-									   (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
-									{
-										image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform());
-									}
-								}
-
-								if(this->isInfoDataFilled() && info)
-								{
-									info->refCorners[oi] = lastCornersKept[i];
-									info->newCorners[oi] = newCornersKept[i];
-								}
-								++oi;
-							}
-						}
-						++flowInliers;
-					}
-				}
-				objectPoints.resize(oi);
-				imagePoints.resize(oi);
-				image3DPoints.resize(oi);
-				UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
-
-				if(this->isInfoDataFilled() && info)
-				{
-					info->refCorners.resize(oi);
-					info->newCorners.resize(oi);
-				}
-
-				correspondences = oi;
+				correspondences = refCornersKept.size();
 
 				if(correspondences >= this->getMinInliers())
 				{
 					//PnPRansac
-					cv::Mat K = (cv::Mat_<double>(3,3) <<
-						data.fx(), 0, data.cx(),
-						0, data.fx(), data.cy(),
-						0, 0, 1);
-					Transform guess = (data.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<int> inliersV;
-					cv::solvePnPRansac(objectPoints,
-							imagePoints,
+					cv::solvePnPRansac(
+							objectPointsKept,
+							newCornersKept,
 							K,
 							cv::Mat(),
 							rvec,
@@ -335,43 +259,25 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 							true,
 							this->getIterations(),
 							this->getPnPReprojError(),
-							0,
+#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())
 					{
-						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));
-
 						// make it incremental
-						output = (data.localTransform() * pnp).inverse();
-
-						UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
-
-						// compute variance (like in PCL computeVariance() method of sac_model.h)
-						std::vector<float> errorSqrdDists(inliersV.size());
-						int ii=0;
-						for(unsigned int i=0; i<inliersV.size(); ++i)
-						{
-							pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
-							if(pcl::isFinite(newPt))
-							{
-								newPt = util3d::transformPoint(newPt, output);
-								const cv::Point3f & objPt = objectPoints[inliersV[i]];
-								errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-							}
-						}
-						errorSqrdDists.resize(ii);
-						if(errorSqrdDists.size())
-						{
-							std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-							double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-							variance = 2.1981 * median_error_sqr;
-						}
+						output = (localTransform * pnp).inverse();
+						variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
 					}
 					else
 					{
@@ -390,57 +296,80 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			}
 			else
 			{
-				UDEBUG("Getting correspondences begin");
 				// Get 3D correspondences
-				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
-				correspondencesLast->resize(statusLast.size());
-				correspondencesNew->resize(statusLast.size());
-				int oi = 0;
+				correspondencesRef->resize(newCornersKept.size());
+				correspondencesNew->resize(newCornersKept.size());
 				if(this->isInfoDataFilled() && info)
 				{
-					info->refCorners.resize(statusLast.size());
-					info->newCorners.resize(statusLast.size());
+					info->refCorners.resize(newCornersKept.size());
+					info->newCorners.resize(newCornersKept.size());
 				}
-				for(unsigned int i=0; i<statusLast.size(); ++i)
+				int oi = 0;
+				if(!data.rightRaw().empty())
 				{
-					if(statusLast[i] && statusNew[i])
-					{
-						float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
-						float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
-						float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
-						float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
-						if(lastDisparity > 0.0f && newDisparity > 0.0f &&
-							lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_)
-						{
-							pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
-									lastCornersKept[i],
-									lastDisparity,
-									data.cx(), data.cy(), data.fx(), data.baseline());
-							pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
-									newCornersKept[i],
-									newDisparity,
-									data.cx(), data.cy(), data.fx(), data.baseline());
+					// 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_);
 
-							if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
-							   pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
+					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!
-								lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
-								newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
-								correspondencesLast->at(oi) = lastPt3D;
-								correspondencesNew->at(oi) = newPt3D;
+								correspondencesRef->at(oi) = refCorners3DKept->at(i);
+								correspondencesNew->at(oi) = util3d::transformPoint(pt, localTransform);
+
 								if(this->isInfoDataFilled() && info)
 								{
-									info->refCorners[oi] = lastCornersKept[i];
+									info->refCorners[oi] = refCornersKept[i];
 									info->newCorners[oi] = newCornersKept[i];
 								}
 								++oi;
 							}
 						}
 					}
-				}// end loop
-				correspondencesLast->resize(oi);
+				}
+				correspondencesRef->resize(oi);
 				correspondencesNew->resize(oi);
 				if(this->isInfoDataFilled() && info)
 				{
@@ -448,8 +377,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 					info->newCorners.resize(oi);
 				}
 				correspondences = oi;
-				refCorners3D_ = correspondencesNew;
-				UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
+				UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
 
 				if(correspondences >= this->getMinInliers())
 				{
@@ -457,7 +385,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 					UTimer timerRANSAC;
 					Transform t = util3d::transformFromXYZCorrespondences(
 							correspondencesNew,
-							correspondencesLast,
+							correspondencesRef,
 							this->getInlierDistance(),
 							this->getIterations(),
 							this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
@@ -499,11 +427,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		// Copy or generate new keypoints
 		if(data.keypoints().size())
 		{
-			newCorners.resize(data.keypoints().size());
-			for(unsigned int i=0; i<data.keypoints().size(); ++i)
-			{
-				newCorners[i] = data.keypoints().at(i).pt;
-			}
+			cv::KeyPoint::convert(data.keypoints(), newCorners);
 		}
 		else
 		{
@@ -528,11 +452,83 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			}
 		}
 
-		if((int)newCorners.size() > this->getMinInliers())
+		if((int)newCorners.size() >= this->getMinInliers())
 		{
-			refFrame_ = newLeftFrame;
-			refRightFrame_ = newRightFrame;
-			refCorners_ = newCorners;
+			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
 		{
@@ -562,394 +558,4 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 	return output;
 }
 
-Transform OdometryOpticalFlow::computeTransformRGBD(
-		const SensorData & data,
-		OdometryInfo * info)
-{
-	UTimer timer;
-	Transform output;
-
-	double variance = 0;
-	int inliers = 0;
-	int correspondences = 0;
-
-	cv::Mat newFrame;
-	// convert to grayscale
-	if(data.image().channels() > 1)
-	{
-		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
-	}
-	else
-	{
-		newFrame = data.image().clone();
-	}
-
-	std::vector<cv::Point2f> newCorners;
-	if(!refFrame_.empty() &&
-		(int)refCorners_.size() >= this->getMinInliers() &&
-		(int)refCorners3D_->size() >= this->getMinInliers())
-	{
-		std::vector<unsigned char> status;
-		std::vector<float> err;
-		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-		cv::calcOpticalFlowPyrLK(
-				refFrame_,
-				newFrame,
-				refCorners_,
-				newCorners,
-				status,
-				err,
-				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
-				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
-		UDEBUG("cv::calcOpticalFlowPyrLK() end");
-
-		if(this->isPnPEstimationUsed())
-		{
-			// find correspondences
-			if(this->isInfoDataFilled() && info)
-			{
-				info->refCorners.resize(refCorners_.size());
-				info->newCorners.resize(refCorners_.size());
-			}
-
-			UASSERT(refCorners_.size() == refCorners3D_->size());
-			UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
-			int flowInliers = 0;
-			std::vector<cv::Point3f> objectPoints(refCorners_.size());
-			std::vector<cv::Point2f> imagePoints(refCorners_.size());
-			std::vector<pcl::PointXYZ> image3DPoints(refCorners_.size());
-			int oi=0;
-			float bad_point = std::numeric_limits<float>::quiet_NaN ();
-			for(unsigned int i=0; i<status.size(); ++i)
-			{
-				if(status[i])
-				{
-					if(pcl::isFinite(refCorners3D_->at(i)))
-					{
-						objectPoints[oi].x = refCorners3D_->at(i).x;
-						objectPoints[oi].y = refCorners3D_->at(i).y;
-						objectPoints[oi].z = refCorners3D_->at(i).z;
-						imagePoints[oi] = newCorners.at(i);
-
-						// new 3D points, used to compute variance
-						image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
-						if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
-						   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
-						{
-							pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
-									data.cx(), data.cy(), data.fx(), data.fy(), true);
-							if(pcl::isFinite(pt) &&
-								(this->getMaxDepth() == 0.0f || (
-								uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
-								uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
-								uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
-							{
-								image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform());
-							}
-						}
-
-						if(this->isInfoDataFilled() && info)
-						{
-							info->refCorners[oi] = refCorners_[i];
-							info->newCorners[oi] = newCorners[i];
-						}
-
-						++oi;
-					}
-					++flowInliers;
-				}
-			}
-			objectPoints.resize(oi);
-			imagePoints.resize(oi);
-			image3DPoints.resize(oi);
-			UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
-
-			if(this->isInfoDataFilled() && info)
-			{
-				info->refCorners.resize(oi);
-				info->newCorners.resize(oi);
-			}
-
-			correspondences = oi;
-
-			if(correspondences >= this->getMinInliers())
-			{
-				//PnPRansac
-				cv::Mat K = (cv::Mat_<double>(3,3) <<
-					data.fx(), 0, data.cx(),
-					0, data.fy(), data.cy(),
-					0, 0, 1);
-				Transform guess = (data.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<int> inliersV;
-				cv::solvePnPRansac(objectPoints,
-						imagePoints,
-						K,
-						cv::Mat(),
-						rvec,
-						tvec,
-						true,
-						this->getIterations(),
-						this->getPnPReprojError(),
-						0,
-						inliersV,
-						this->getPnPFlags());
-
-				inliers = (int)inliersV.size();
-				if((int)inliersV.size() >= this->getMinInliers())
-				{
-					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));
-
-					// make it incremental
-					output = (data.localTransform() * pnp).inverse();
-
-					UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
-
-					// compute variance (like in PCL computeVariance() method of sac_model.h)
-					std::vector<float> errorSqrdDists(inliersV.size());
-					int ii=0;
-					for(unsigned int i=0; i<inliersV.size(); ++i)
-					{
-						pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
-						if(pcl::isFinite(newPt))
-						{
-							newPt = util3d::transformPoint(newPt, output);
-							const cv::Point3f & objPt = objectPoints[inliersV[i]];
-							errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
-						}
-					}
-					errorSqrdDists.resize(ii);
-					if(errorSqrdDists.size())
-					{
-						std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-						double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-						variance = 2.1981 * median_error_sqr;
-					}
-				}
-				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
-		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
-			correspondencesLast->resize(refCorners_.size());
-			correspondencesNew->resize(refCorners_.size());
-			int oi=0;
-
-			if(this->isInfoDataFilled() && info)
-			{
-				info->refCorners.resize(refCorners_.size());
-				info->newCorners.resize(refCorners_.size());
-			}
-
-			UASSERT(refCorners_.size() == refCorners3D_->size());
-			UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
-			int flowInliers = 0;
-			for(unsigned int i=0; i<status.size(); ++i)
-			{
-				if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
-					uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
-					uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
-				{
-					pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
-							data.cx(), data.cy(), data.fx(), data.fy(), true);
-					if(pcl::isFinite(pt) &&
-						(this->getMaxDepth() == 0.0f || (
-						uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
-						uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
-						uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
-					{
-						pt = util3d::transformPoint(pt, data.localTransform());
-						correspondencesLast->at(oi) = refCorners3D_->at(i);
-						correspondencesNew->at(oi) = pt;
-
-						if(this->isInfoDataFilled() && info)
-						{
-							info->refCorners[oi] = refCorners_[i];
-							info->newCorners[oi] = newCorners[i];
-						}
-
-						++oi;
-					}
-					++flowInliers;
-				}
-				else if(status[i])
-				{
-					++flowInliers;
-				}
-			}
-			UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
-
-			if(this->isInfoDataFilled() && info)
-			{
-				info->refCorners.resize(oi);
-				info->newCorners.resize(oi);
-			}
-			correspondencesLast->resize(oi);
-			correspondencesNew->resize(oi);
-			correspondences = oi;
-			if(correspondences >= this->getMinInliers())
-			{
-				std::vector<int> inliersV;
-				UTimer timerRANSAC;
-				output = util3d::transformFromXYZCorrespondences(
-						correspondencesNew,
-						correspondencesLast,
-						this->getInlierDistance(),
-						this->getIterations(),
-						this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
-						&inliersV,
-						&variance);
-				UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
-
-				inliers = (int)inliersV.size();
-				if(inliers < this->getMinInliers())
-				{
-					output.setNull();
-					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())
-		{
-			newCorners.resize(data.keypoints().size());
-			for(unsigned int i=0; i<data.keypoints().size(); ++i)
-			{
-				newCorners[i] = data.keypoints().at(i).pt;
-			}
-		}
-		else
-		{
-			// generate kpts
-			std::vector<cv::KeyPoint> newKtps;
-			cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
-			newKtps = feature2D_->generateKeypoints(newFrame, roi);
-			Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
-
-			if(newKtps.size())
-			{
-				cv::KeyPoint::convert(newKtps, newCorners);
-
-				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
-				{
-					cv::cornerSubPix(newFrame, newCorners,
-						cv::Size( subPixWinSize_, subPixWinSize_ ),
-						cv::Size( -1, -1 ),
-						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
-				}
-			}
-		}
-
-		if((int)newCorners.size() > this->getMinInliers())
-		{
-			// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
-			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;
-			for(unsigned int i=0; i<newCorners.size(); ++i)
-			{
-				if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
-				   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
-				{
-					pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
-							data.cx(), data.cy(), data.fx(), data.fy(), true);
-					if(pcl::isFinite(pt) &&
-						(this->getMaxDepth() == 0.0f || (
-						uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
-						uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
-						uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
-					{
-						pt = util3d::transformPoint(pt, data.localTransform());
-						newCorners3D->at(oi) = pt;
-						newCornersFiltered[oi] = newCorners[i];
-						++oi;
-					}
-				}
-			}
-			newCornersFiltered.resize(oi);
-			newCorners3D->resize(oi);
-			if((int)newCornersFiltered.size() > this->getMinInliers())
-			{
-				refFrame_ = newFrame;
-				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, variance=%f, new corners=%d",
-			timer.elapsed(),
-			output.isNull()?"true":"false",
-			inliers,
-			correspondences,
-			variance,
-			(int)newCorners.size());
-	return output;
-}
-
 } // namespace rtabmap

corelib/src/OdometryThread.cpp

@@ -34,8 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OdometryThread::OdometryThread(Odometry * odometry) :
+OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
 	_odometry(odometry),
+	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false)
 {
 	UASSERT(_odometry != 0);
@@ -59,7 +60,7 @@ void OdometryThread::handleEvent(UEvent * event)
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			CameraEvent * cameraEvent = (CameraEvent*)event;
-			if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
+			if(cameraEvent->getCode() == CameraEvent::kCodeData)
 			{
 				this->addData(cameraEvent->data());
 			}
@@ -92,21 +93,21 @@ void OdometryThread::mainLoop()
 	}
 
 	SensorData data;
-	getData(data);
-	if(data.isValid())
+	if(getData(data))
 	{
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
-		data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(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)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryBOW*>(_odometry) == 0)
 	{
-		if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		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;
@@ -114,7 +115,8 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
-		if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
+		// 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;
@@ -124,8 +126,13 @@ void OdometryThread::addData(const SensorData & data)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		notify = !_dataBuffer.isValid();
-		_dataBuffer = data;
+		_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();
 
@@ -135,18 +142,21 @@ void OdometryThread::addData(const SensorData & data)
 	}
 }
 
-void OdometryThread::getData(SensorData & data)
+bool OdometryThread::getData(SensorData & data)
 {
+	bool dataFilled = false;
 	_dataAdded.acquire();
 	_dataMutex.lock();
 	{
-		if(_dataBuffer.isValid())
+		if(!_dataBuffer.empty())
 		{
-			data = _dataBuffer;
-			_dataBuffer = SensorData();
+			data = _dataBuffer.front();
+			_dataBuffer.pop_front();
+			dataFilled = true;
 		}
 	}
 	_dataMutex.unlock();
+	return dataFilled;
 }
 
 } // namespace rtabmap

corelib/src/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/src/RtabmapThread.cpp

@@ -46,6 +46,7 @@ namespace rtabmap {
 RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
+		_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		_frameRateTimer(new UTimer()),
 		_rtabmap(rtabmap),
 		_paused(false),
@@ -106,13 +107,17 @@ 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;
@@ -122,54 +127,30 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, std::string> labels;
 	std::map<int, std::vector<unsigned char> > userDatas;
 
-	_rtabmap->get3DMap(signatures,
-			poses,
-			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
-			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,
-			stamps,
-			labels,
-			userDatas));
+			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;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-
-	_rtabmap->getGraph(poses,
-			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas,
-			optimized,
-			full);
-
-	this->post(new RtabmapEvent3DMap(signatures,
-			poses,
-			constraints,
-			mapIds,
-			stamps,
-			labels,
-			userDatas));
-}
-
-
 void RtabmapThread::mainLoopKill()
 {
 	this->clearBufferedData();
@@ -196,7 +177,7 @@ void RtabmapThread::mainLoop()
 	_stateMutex.unlock();
 
 	int id = 0;
-	std::vector<unsigned char> userData;
+	cv::Mat userData;
 	switch(state)
 	{
 	case kStateDetecting:
@@ -206,6 +187,7 @@ void RtabmapThread::mainLoop()
 		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"));
@@ -213,6 +195,7 @@ void RtabmapThread::mainLoop()
 	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);
@@ -235,32 +218,27 @@ 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();
@@ -269,16 +247,16 @@ void RtabmapThread::mainLoop()
 		_userDataMutex.lock();
 		{
 			userData = _userData;
-			_userData.clear();
+			_userData = cv::Mat();
 		}
 		_userDataMutex.unlock();
 		_rtabmap->setUserData(0, userData);
 		break;
 	case kStateSettingGoal:
-		id = atoi(parameters.at("goal_id").c_str());
-		if(id == 0 && !parameters.at("goal_label").empty() && _rtabmap->getMemory())
+		id = atoi(parameters.at("id").c_str());
+		if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
 		{
-			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
+			id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
 		}
 		if(id <= 0 || !_rtabmap->computePath(id, true))
 		{
@@ -286,6 +264,15 @@ void RtabmapThread::mainLoop()
 		}
 		this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
 		break;
+	case kStateCancellingGoal:
+		_rtabmap->clearPath();
+		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;
@@ -299,18 +286,18 @@ 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
 		{
@@ -351,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)
@@ -377,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)
@@ -424,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)
 		{
@@ -465,11 +420,27 @@ void RtabmapThread::handleEvent(UEvent* event)
 		else if(cmd == RtabmapEventCmd::kCmdGoal)
 		{
 			ULOGGER_DEBUG("CMD_GOAL");
+			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
 			ParametersMap param;
-			param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
-			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
+			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);
@@ -487,13 +458,12 @@ void RtabmapThread::handleEvent(UEvent* event)
 //============================================================
 void RtabmapThread::process()
 {
-	SensorData data;
-	getData(data);
-	if(data.isValid() && _state.empty())
+	OdometryEvent data;
+	if(_state.empty() && getData(data))
 	{
 		if(_rtabmap->getMemory())
 		{
-			if(_rtabmap->process(data))
+			if(_rtabmap->process(data.data(), data.pose(), data.covariance()))
 			{
 				Statistics stats = _rtabmap->getStatistics();
 				stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size());
@@ -508,17 +478,19 @@ void RtabmapThread::process()
 	}
 }
 
-void RtabmapThread::addData(const SensorData & sensorData)
+void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
-		if(!sensorData.isValid())
+		bool ignoreFrame = false;
+		if(_rate>0.0f)
 		{
-			ULOGGER_ERROR("data not valid !?");
-			return;
+			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
+			{
+				ignoreFrame = true;
+			}
 		}
-
-		if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
+		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity())
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
@@ -526,41 +498,57 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			_transVariance = 0;
 		}
 
-		lastPose_ = sensorData.pose();
-		if(sensorData.poseRotVariance() > _rotVariance)
+		if(ignoreFrame && !_createIntermediateNodes)
 		{
-			_rotVariance = sensorData.poseRotVariance();
+			return;
 		}
-		if(sensorData.poseTransVariance() > _transVariance)
+		else if(!ignoreFrame)
 		{
-			_transVariance = sensorData.poseTransVariance();
+			_frameRateTimer->start();
 		}
 
-		if(_rate>0.0f)
+		lastPose_ = odomEvent.pose();
+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		if(maxRotVar > _rotVariance)
 		{
-			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
-			{
-				return;
-			}
+			_rotVariance = maxRotVar;
+		}
+		if(maxTransVar > _transVariance)
+		{
+			_transVariance = maxTransVar;
 		}
-		_frameRateTimer->start();
 
 		bool notify = true;
 		_dataMutex.lock();
 		{
-			_dataBuffer.push_back(sensorData);
 			if(_rotVariance <= 0)
 			{
-				_rotVariance = 1.0f;
+				_rotVariance = 1.0;
 			}
 			if(_transVariance <= 0)
 			{
-				_transVariance = 1.0f;
+				_transVariance = 1.0;
 			}
-			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
+			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() > (unsigned int)_dataBufferMaxSize)
+			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();
@@ -576,7 +564,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 	}
 }
 
-void RtabmapThread::getData(SensorData & image)
+bool RtabmapThread::getData(OdometryEvent & data)
 {
 	ULOGGER_DEBUG("");
 
@@ -584,28 +572,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,138 +27,542 @@ 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() :
-	_id(0),
-	_stamp(0.0),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0)
+		_id(0),
+		_stamp(0.0),
+		_laserScanMaxPts(0)
 {
 }
 
-SensorData::SensorData(const cv::Mat & image,
-	  int id,
-	  double stamp,
-	  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_fx(0.0f),
-	_fyOrBaseline(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_localTransform(Transform::getIdentity()),
-	_poseRotVariance(1.0f),
-	_poseTransVariance(1.0f),
-	_laserScanMaxPts(0),
-	_userData(userData)
+// Appearance-only constructor
+SensorData::SensorData(
+		const cv::Mat & image,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_laserScanMaxPts(0)
 {
-	UASSERT(image.empty() ||
-			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 & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(0),
-	_userData(userData)
+// 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),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
-	UASSERT(image.empty() ||
-			image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.empty() ||
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!_localTransform.isNull());
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+	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 & laserScan,
-		  int laserScanMaxPts,
-		  const cv::Mat & image,
-		  const cv::Mat & depthOrRightImage,
-		  float fx,
-		  float fyOrBaseline,
-		  float cx,
-		  float cy,
-		  const Transform & localTransform,
-		  const Transform & pose,
-		  float poseRotVariance,
-		  float poseTransVariance,
-		  int id,
-		  double stamp,
-		  const std::vector<unsigned char> & userData) :
-	_image(image),
-	_id(id),
-	_stamp(stamp),
-	_depthOrRightImage(depthOrRightImage),
-	_laserScan(laserScan),
-	_fx(fx),
-	_fyOrBaseline(fyOrBaseline),
-	_cx(cx),
-	_cy(cy),
-	_pose(pose),
-	_localTransform(localTransform),
-	_poseRotVariance(poseRotVariance),
-	_poseTransVariance(poseTransVariance),
-	_laserScanMaxPts(laserScanMaxPts),
-	_userData(userData)
+// 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),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
-	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
-	UASSERT(image.empty() ||
-			image.type() == CV_8UC1 || // Mono
-			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.empty() ||
-			depthOrRightImage.type() == CV_32FC1 || // Depth in meter
-			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
-			depthOrRightImage.type() == CV_8U);     // Right stereo image
-	UASSERT(!_localTransform.isNull());
-	UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
+	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,
+		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),
+		_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),
+		_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,
+		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),
+		_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),
+		_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,
+		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),
+		_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

@@ -39,17 +39,11 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
-	_stamp(0.0),
-	_weight(-1),
+	_weight(0),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false),
-	_fx(0.0f),
-	_fy(0.0f),
-	_cx(0.0f),
-	_cy(0.0f),
-	_laserScanMaxPts(0)
+	_enabled(false)
 {
 }
 
@@ -59,42 +53,25 @@ Signature::Signature(
 		int weight,
 		double stamp,
 		const std::string & label,
-		const std::multimap<int, cv::KeyPoint> & words,
-		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
 		const Transform & pose,
-		const std::vector<unsigned char> & userData,
-		const cv::Mat & laserScanCompressed, // 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,
-		int laserScanMaxPts) :
+		const SensorData & sensorData):
 	_id(id),
 	_mapId(mapId),
 	_stamp(stamp),
 	_weight(weight),
 	_label(label),
-	_userData(userData),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_words(words),
 	_enabled(false),
-	_imageCompressed(imageCompressed),
-	_depthCompressed(depthCompressed),
-	_laserScanCompressed(laserScanCompressed),
-	_fx(fx),
-	_fy(fy),
-	_cx(cx),
-	_cy(cy),
 	_pose(pose),
-	_localTransform(localTransform),
-	_words3(words3),
-	_laserScanMaxPts(laserScanMaxPts)
+	_sensorData(sensorData)
 {
+	if(_sensorData.id() == 0)
+	{
+		_sensorData.setId(id);
+	}
+	UASSERT(_sensorData.id() == _id);
 }
 
 Signature::~Signature()
@@ -102,18 +79,6 @@ Signature::~Signature()
 	//UDEBUG("id=%d", _id);
 }
 
-void Signature::setUserData(const std::vector<unsigned char> & data)
-{
-	if(!_userData.empty() && !data.empty())
-	{
-		UWARN("Node %d: Current user data (%d bytes) overwritten by new data (%d bytes)",
-				_id, (int)_userData.size(), (int)data.size());
-	}
-
-	_modified = true;
-	_userData = data;
-}
-
 void Signature::addLinks(const std::list<Link> & links)
 {
 	for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
@@ -239,25 +204,9 @@ 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;
-}
-
-float Signature::getDepthFx() const {return getFx();}
-float Signature::getDepthFy() const {return getFy();}
-float Signature::getDepthCx() const {return getCx();}
-float Signature::getDepthCy() const {return getCy();}
-
-void Signature::getPoseVariance(float & rotVariance, float & transVariance) const
-{
-	rotVariance = 1.0f;
-	transVariance = 1.0f;
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
 	if(_links.size())
 	{
 		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -267,110 +216,13 @@ void Signature::getPoseVariance(float & rotVariance, float & transVariance) cons
 				//Assume the first neighbor to be the backward neighbor link
 				if(iter->second.to() < iter->second.from())
 				{
-					rotVariance = iter->second.rotVariance();
-					transVariance = iter->second.transVariance();
+					covariance = iter->second.infMatrix().inv();
 					break;
 				}
 			}
 		}
 	}
-}
-
-SensorData Signature::toSensorData()
-{
-	this->uncompressData();
-	float rotVariance = 1.0f;
-	float transVariance = 1.0f;
-	this->getPoseVariance(rotVariance, transVariance);
-
-	return SensorData(_laserScanRaw,
-			_laserScanMaxPts,
-			_imageRaw,
-			_depthRaw,
-			_fx,
-			_fy,
-			_cx,
-			_cy,
-			_localTransform,
-			_pose,
-			rotVariance,
-			transVariance,
-			_id,
-			_stamp,
-			_userData);
-}
-
-void Signature::uncompressData()
-{
-	uncompressData(&_imageRaw, &_depthRaw, &_laserScanRaw);
-}
-
-void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
-{
-	uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
-	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
-	{
-		_imageRaw = *imageRaw;
-	}
-	if(depthRaw && !depthRaw->empty() && _depthRaw.empty())
-	{
-		_depthRaw = *depthRaw;
-	}
-	if(laserScanRaw && !laserScanRaw->empty() && _laserScanRaw.empty())
-	{
-		_laserScanRaw = *laserScanRaw;
-	}
-}
-
-void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
-{
-	if(imageRaw)
-	{
-		*imageRaw = _imageRaw;
-	}
-	if(depthRaw)
-	{
-		*depthRaw = _depthRaw;
-	}
-	if(laserScanRaw)
-	{
-		*laserScanRaw = _laserScanRaw;
-	}
-	if( (imageRaw && imageRaw->empty()) ||
-		(depthRaw && depthRaw->empty()) ||
-		(laserScanRaw && laserScanRaw->empty()))
-	{
-		rtabmap::CompressionThread ctImage(_imageCompressed, true);
-		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
-		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
-		if(imageRaw && imageRaw->empty())
-		{
-			ctImage.start();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			ctDepth.start();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			ctLaserScan.start();
-		}
-		ctImage.join();
-		ctDepth.join();
-		ctLaserScan.join();
-		if(imageRaw && imageRaw->empty())
-		{
-			*imageRaw = ctImage.getUncompressedData();
-		}
-		if(depthRaw && depthRaw->empty())
-		{
-			*depthRaw = ctDepth.getUncompressedData();
-		}
-		if(laserScanRaw && laserScanRaw->empty())
-		{
-			*laserScanRaw = ctLaserScan.getUncompressedData();
-		}
-	}
+	return covariance;
 }
 
 } //namespace rtabmap

corelib/src/Transform.cpp

@@ -31,44 +31,33 @@ 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)
@@ -79,46 +68,46 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
 
 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()
@@ -145,16 +134,17 @@ Transform Transform::inverse() const
 
 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
@@ -215,7 +205,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
@@ -239,18 +229,18 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 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],
+	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],
+	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;
 }

corelib/src/VWDictionary.cpp

@@ -34,7 +34,15 @@ 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 <fstream>
 #include <string>
@@ -220,11 +228,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)
 		{
@@ -466,6 +489,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		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)
@@ -478,6 +502,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
 		{
@@ -681,6 +722,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 			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)
@@ -693,6 +735,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
 			{

corelib/src/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/src/resources/DatabaseSchema.sql.in

@@ -20,29 +20,18 @@ CREATE TABLE Node (
 	stamp FLOAT,
 	pose BLOB,
 	label TEXT,
-	user_data BLOB,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
 
-CREATE TABLE Image (
+CREATE TABLE Data (
 	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (RGB)
-	time_enter DATE,
-	PRIMARY KEY (id)
-);
-
--- TODO: Merge "Image" and "Depth" tables to "Data" table.
-CREATE TABLE Depth (
-	id INTEGER NOT NULL,
-	data BLOB,       -- compressed image (Depth or Right image)
-	fx FLOAT,
-	fy FLOAT,        -- baseline if stereo
-	cx FLOAT,
-	cy FLOAT,
-	local_transform BLOB,
-	data2d BLOB,     -- compressed data (Laser scan)
-	data2d_max_pts INTEGER,   -- Laser scan max points
+	image BLOB,               -- compressed image (Grayscale or RGB)
+	depth BLOB,               -- compressed image (Depth or Right image)
+	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_max_pts INTEGER,     -- Laser scan max points
+	user_data BLOB,           -- compressed data (User data)
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

corelib/src/util2d.cpp

@@ -58,7 +58,8 @@ cv::Mat disparityFromStereoImages(
 	{
 		leftMono = leftImage;
 	}
-
+	cv::Mat disparity;
+#if CV_MAJOR_VERSION < 3
 	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
 	stereo.state->SADWindowSize = 15;
 	stereo.state->minDisparity = 0;
@@ -69,8 +70,20 @@ cv::Mat disparityFromStereoImages(
 	stereo.state->textureThreshold = 10;
 	stereo.state->speckleWindowSize = 100;
 	stereo.state->speckleRange = 4;
-	cv::Mat disparity;
 	stereo(leftMono, rightImage, disparity, CV_16SC1);
+#else
+	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
+	stereo->setBlockSize(15);
+	stereo->setMinDisparity(0);
+	stereo->setNumDisparities(64);
+	stereo->setPreFilterSize(9);
+	stereo->setPreFilterCap(31);
+	stereo->setUniquenessRatio(15);
+	stereo->setTextureThreshold(10);
+	stereo->setSpeckleWindowSize(100);
+	stereo->setSpeckleRange(4);
+	stereo->compute(leftMono, rightImage, disparity);
+#endif
 	return disparity;
 }
 
@@ -162,7 +175,7 @@ cv::Mat disparityFromStereoCorrespondences(
 		{
 			float d = leftCorners[i].x - rightCorners[i].x;
 			float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
-			if(d > 0.0f && slope < maxSlope)
+			if(d > 0.0f && (maxSlope <= 0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxSlope))
 			{
 				disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
 			}
@@ -210,7 +223,7 @@ float getDepth(
 
 	if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
 	{
-		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
+		UDEBUG("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
 				x,u,y,v,depthImage.cols, depthImage.rows);
 		return 0;
 	}
@@ -228,7 +241,7 @@ float getDepth(
 	int u_end = std::min(u+1, depthImage.cols-1);
 	int v_end = std::min(v+1, depthImage.rows-1);
 
-	float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
+	float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u);
 	if(depth!=0.0f && uIsFinite(depth))
 	{
 		if(smoothing)
@@ -241,7 +254,7 @@ float getDepth(
 				{
 					if(!(uu == u && vv == v))
 					{
-						float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
+						float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
 						// ignore if not valid or depth difference is too high
 						if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
 						{

corelib/src/util3d.cpp

@@ -27,10 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap
@@ -351,12 +353,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
 	UASSERT(imageDisparity.rows % decimation == 0);
 	UASSERT(imageDisparity.cols % decimation == 0);
+	UASSERT(decimation >= 1);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	if(decimation < 1)
-	{
-		return cloud;
-	}
 
 	//cloud.header = cameraInfo.header;
 	cloud->height = imageDisparity.rows/decimation;
@@ -396,30 +395,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		float fx, float baseline,
 		int decimation)
 {
+	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
 	UASSERT(imageRgb.rows == imageDisparity.rows &&
 			imageRgb.cols == imageDisparity.cols &&
 			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
-	UASSERT(imageDisparity.rows % decimation == 0);
-	UASSERT(imageDisparity.cols % decimation == 0);
+	UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
+	UASSERT(decimation >= 1);
+	UASSERT(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0);
+
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	if(decimation < 1)
-	{
-		return cloud;
-	}
 
 	bool mono;
 	if(imageRgb.channels() == 3) // BGR
 	{
 		mono = false;
 	}
-	else if(imageRgb.channels() == 1) // Mono
+	else // Mono
 	{
 		mono = true;
 	}
-	else
-	{
-		return cloud;
-	}
 
 	//cloud.header = cameraInfo.header;
 	cloud->height = imageRgb.rows/decimation;
@@ -463,25 +457,422 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		float fx, float baseline,
 		int decimation)
 {
+	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
+	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
+	UASSERT(imageLeft.rows == imageRight.rows &&
+			imageLeft.cols == imageRight.cols);
+	UASSERT(decimation >= 1);
+
+	cv::Mat leftColor = imageLeft;
+	cv::Mat rightMono = imageRight;
+
+	if(leftColor.rows % decimation != 0 ||
+	   leftColor.cols % decimation != 0)
+	{
+		leftColor = util2d::decimate(leftColor, decimation);
+		rightMono = util2d::decimate(rightMono, decimation);
+		fx /= float(decimation);
+		cx /= float(decimation);
+		cy /= float(decimation);
+		decimation = 1;
+	}
 
 	cv::Mat leftMono;
-	if(imageLeft.channels() == 3)
+	if(leftColor.channels() == 3)
 	{
-		cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
+		cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
 	}
 	else
 	{
-		leftMono = imageLeft;
+		leftMono = leftColor;
 	}
+
 	return cloudFromDisparityRGB(
-			imageLeft,
-			util2d::disparityFromStereoImages(leftMono, imageRight),
+			leftColor,
+			util2d::disparityFromStereoImages(leftMono, rightMono),
 			cx, cy,
 			fx, baseline,
 			decimation);
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+	{
+		//depth
+		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+		cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+		{
+			if(sensorData.cameraModels()[i].isValid())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
+						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i].cy(),
+						sensorData.cameraModels()[i].fx(),
+						sensorData.cameraModels()[i].fy(),
+						decimation);
+
+				if(tmp->size())
+				{
+					bool filtered = false;
+					if(tmp->size() && maxDepth)
+					{
+						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+						filtered = true;
+					}
+
+					if(tmp->size() && voxelSize)
+					{
+						tmp = util3d::voxelize(tmp, voxelSize);
+						filtered = true;
+					}
+
+					if(tmp->size() && samples)
+					{
+						tmp = util3d::sampling(tmp, samples);
+						filtered = true;
+					}
+
+					if(tmp->size() && !filtered)
+					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
+					}
+
+					if(tmp->size())
+					{
+						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+					}
+
+					*cloud += *tmp;
+				}
+			}
+			else
+			{
+				UERROR("Camera model %d is invalid", i);
+			}
+		}
+
+		if(cloud->size() && voxelSize)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize);
+		}
+	}
+	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+	{
+		//stereo
+		UASSERT(sensorData.rightRaw().type() == CV_8UC1);
+
+		cv::Mat leftMono;
+		if(sensorData.imageRaw().channels() == 3)
+		{
+			cv::cvtColor(sensorData.imageRaw(), leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = sensorData.imageRaw();
+		}
+		cloud = cloudFromDisparity(
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				sensorData.stereoCameraModel().left().cx(),
+				sensorData.stereoCameraModel().left().cy(),
+				sensorData.stereoCameraModel().left().fx(),
+				sensorData.stereoCameraModel().baseline(),
+				decimation);
+
+		if(cloud->size())
+		{
+			bool filtered = false;
+			if(cloud->size() && maxDepth)
+			{
+				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+				filtered = true;
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+				filtered = true;
+			}
+
+			if(cloud->size() && !filtered)
+			{
+				cloud = util3d::removeNaNFromPointCloud(cloud);
+			}
+
+			if(cloud->size())
+			{
+				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float voxelSize,
+		int samples)
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+
+	if(!sensorData.imageRaw().empty())
+	{
+		if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+		{
+			//depth
+			UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
+			UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
+			int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+			cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+			{
+				if(sensorData.cameraModels()[i].isValid())
+				{
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
+							cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
+							cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+							sensorData.cameraModels()[i].cx(),
+							sensorData.cameraModels()[i].cy(),
+							sensorData.cameraModels()[i].fx(),
+							sensorData.cameraModels()[i].fy(),
+							decimation);
+
+					if(tmp->size())
+					{
+						bool filtered = false;
+						if(tmp->size() && maxDepth)
+						{
+							tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+							filtered = true;
+						}
+
+						if(tmp->size() && voxelSize)
+						{
+							tmp = util3d::voxelize(tmp, voxelSize);
+							filtered = true;
+						}
+
+						if(tmp->size() && samples)
+						{
+							tmp = util3d::sampling(tmp, samples);
+							filtered = true;
+						}
+
+						if(tmp->size() && !filtered)
+						{
+							tmp = util3d::removeNaNFromPointCloud(tmp);
+						}
+
+						if(tmp->size())
+						{
+							tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+						}
+
+						*cloud += *tmp;
+					}
+				}
+				else
+				{
+					UERROR("Camera model %d is invalid", i);
+				}
+			}
+
+			if(cloud->size() && voxelSize)
+			{
+				cloud = util3d::voxelize(cloud, voxelSize);
+			}
+		}
+		else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+		{
+			//stereo
+			cloud = cloudFromStereoImages(sensorData.imageRaw(),
+					sensorData.rightRaw(),
+					sensorData.stereoCameraModel().left().cx(),
+					sensorData.stereoCameraModel().left().cy(),
+					sensorData.stereoCameraModel().left().fx(),
+					sensorData.stereoCameraModel().baseline(),
+					decimation);
+
+			if(cloud->size())
+			{
+				bool filtered = false;
+				if(cloud->size() && maxDepth)
+				{
+					cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+					filtered = true;
+				}
+
+				if(cloud->size() && voxelSize)
+				{
+					cloud = util3d::voxelize(cloud, voxelSize);
+					filtered = true;
+				}
+
+				if(cloud->size() && !filtered)
+				{
+					cloud = util3d::removeNaNFromPointCloud(cloud);
+				}
+
+				if(cloud->size())
+				{
+					cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
+				}
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth,
+					const Transform & localTransform)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+	UASSERT(!localTransform.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ> scan;
+	int middle = depthImage.rows/2;
+	if(middle)
+	{
+		scan.resize(depthImage.cols);
+		int oi = 0;
+		for(int i=0; i<depthImage.cols; ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
+			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			{
+				if(!localTransform.isIdentity())
+				{
+					pt = util3d::transformPoint(pt, localTransform);
+				}
+				scan[oi++] = pt;
+			}
+		}
+		scan.resize(oi);
+	}
+	return scan;
+}
+
+
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+	}
+	return laserScan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	output->resize(laserScan.cols);
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+	}
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform)
+{
+	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
+	UASSERT(matrix.rows == 1);
+
+	Eigen::Affine3f t = tranform.toEigen3f();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(matrix.cols);
+	if(matrix.channels() == 2)
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
+			cloud->at(i).z = 0.0f;
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	else // channels=3
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
+			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	return cloud;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/util3d_conversions.cpp

@@ -1,142 +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.
-*/
-
-#include "rtabmap/core/util3d_conversions.h"
-
-#include "rtabmap/utilite/ULogger.h"
-#include <pcl/common/transforms.h>
-
-namespace rtabmap
-{
-
-namespace util3d
-{
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
-cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
-{
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
-	for(unsigned int i=0; i<cloud.size(); ++i)
-	{
-		laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
-		laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
-	}
-	return laserScan;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
-{
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	output->resize(laserScan.cols);
-	for(int i=0; i<laserScan.cols; ++i)
-	{
-		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-	}
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
-		const cv::Mat & matrix,
-		const Transform & tranform)
-{
-	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
-	UASSERT(matrix.rows == 1);
-
-	Eigen::Affine3f t = tranform.toEigen3f();
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(matrix.cols);
-	if(matrix.channels() == 2)
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
-			cloud->at(i).z = 0.0f;
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	else // channels=3
-	{
-		for(int i=0; i<matrix.cols; ++i)
-		{
-			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
-			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
-			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
-			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
-		}
-	}
-	return cloud;
-}
-
-}
-
-}

corelib/src/util3d_correspondences.cpp

@@ -314,53 +314,22 @@ void findCorrespondences(
 	}
 }
 
-
-std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
-		const std::multimap<int, cv::KeyPoint> & words1,
-		const std::multimap<int, cv::KeyPoint> & words2)
-{
-	std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
-
-	// Find pairs
-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-	rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
-
-	if(pairs.size() > 7) // 8 min?
-	{
-		// Find fundamental matrix
-		std::vector<uchar> status;
-		cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
-		//ROS_INFO("inliers = %d/%d", uSum(status), pairs.size());
-		if(!fundamentalMatrix.empty())
-		{
-			int i = 0;
-			//int goodCount = 0;
-			for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
-			{
-				if(status[i])
-				{
-					correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
-					//ROS_INFO("inliers kpts %f %f vs %f %f", iter->second.first.pt.x, iter->second.first.pt.y, iter->second.second.pt.x, iter->second.second.pt.y);
-				}
-				++i;
-			}
-		}
-	}
-	return correspondences;
-}
-
 void 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)
+		std::vector<int> * uniqueCorrespondences)
 {
 	std::list<int> ids = uUniqueKeys(words1);
 	// Find pairs
 	inliers1.resize(ids.size());
 	inliers2.resize(ids.size());
+	if(uniqueCorrespondences)
+	{
+		uniqueCorrespondences->resize(ids.size());
+	}
 
 	int oi=0;
 	for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
@@ -375,16 +344,66 @@ void findCorrespondences(
 			   (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
 			   (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
 			{
-				++oi;
 				if(uniqueCorrespondences)
 				{
-					uniqueCorrespondences->insert(*iter);
+					uniqueCorrespondences->at(oi) = *iter;
 				}
+				++oi;
 			}
 		}
 	}
 	inliers1.resize(oi);
 	inliers2.resize(oi);
+	if(uniqueCorrespondences)
+	{
+		uniqueCorrespondences->resize(oi);
+	}
+}
+
+void 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)
+{
+	std::vector<int> ids = uKeys(words1);
+	// Find pairs
+	inliers1.resize(ids.size());
+	inliers2.resize(ids.size());
+	if(correspondences)
+	{
+		correspondences->resize(ids.size());
+	}
+
+	int oi=0;
+	for(std::vector<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		if(words2.find(*iter) != words2.end())
+		{
+			inliers1[oi] = words1.find(*iter)->second;
+			inliers2[oi] = words2.find(*iter)->second;
+			if(pcl::isFinite(inliers1[oi]) &&
+			   pcl::isFinite(inliers2[oi]) &&
+			   (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
+			   (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
+			   (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
+			{
+				if(correspondences)
+				{
+					correspondences->at(oi) = *iter;
+				}
+				++oi;
+			}
+		}
+	}
+	inliers1.resize(oi);
+	inliers2.resize(oi);
+	if(correspondences)
+	{
+		correspondences->resize(oi);
+	}
 }
 
 }

corelib/src/util3d_features.cpp

@@ -44,36 +44,49 @@ namespace rtabmap
 namespace util3d
 {
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & depth,
+		const CameraModel & cameraModel)
+{
+	UASSERT(cameraModel.isValid());
+	std::vector<CameraModel> models;
+	models.push_back(cameraModel);
+	return generateKeypoints3DDepth(keypoints, depth, models);
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & transform)
+		const std::vector<CameraModel> & cameraModels)
 {
 	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
+	UASSERT(cameraModels.size());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	if(!depth.empty())
 	{
+		UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
+		float subImageWidth = depth.cols/cameraModels.size();
 		keypoints3d->resize(keypoints.size());
 		for(unsigned int i=0; i!=keypoints.size(); ++i)
 		{
+			int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
+			UASSERT(cameraIndex < (int)cameraModels.size());
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(
 					depth,
-					keypoints[i].pt.x,
+					keypoints[i].pt.x-subImageWidth*cameraIndex,
 					keypoints[i].pt.y,
-					cx,
-					cy,
-					fx,
-					fy,
+					cameraModels.at(cameraIndex).cx(),
+					cameraModels.at(cameraIndex).cy(),
+					cameraModels.at(cameraIndex).fx(),
+					cameraModels.at(cameraIndex).fy(),
 					true);
 
-			if(!transform.isNull() && !transform.isIdentity())
+			if(pcl::isFinite(pt) &&
+				!cameraModels.at(cameraIndex).localTransform().isNull() &&
+			    !cameraModels.at(cameraIndex).localTransform().isIdentity())
 			{
-				pt = util3d::transformPoint(pt, transform);
+				pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
 			}
 			keypoints3d->at(i) = pt;
 		}
@@ -84,13 +97,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & disparity,
-		float fx,
-		float baseline,
-		float cx,
-		float cy,
-		const Transform & transform)
+		const StereoCameraModel & stereoCameraModel)
 {
 	UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
+	UASSERT(stereoCameraModel.isValid());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
 	keypoints3d->resize(keypoints.size());
 	for(unsigned int i=0; i!=keypoints.size(); ++i)
@@ -98,14 +108,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
 		pcl::PointXYZ pt = util3d::projectDisparityTo3D(
 				keypoints[i].pt,
 				disparity,
-				cx,
-				cy,
-				fx,
-				baseline);
+				stereoCameraModel.left().cx(),
+				stereoCameraModel.left().cy(),
+				stereoCameraModel.left().fx(),
+				stereoCameraModel.baseline());
 
-		if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
+		if(pcl::isFinite(pt) &&
+			!stereoCameraModel.left().localTransform().isNull() &&
+			!stereoCameraModel.left().localTransform().isIdentity())
 		{
-			pt = util3d::transformPoint(pt, transform);
+			pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
 		}
 		keypoints3d->at(i) = pt;
 	}
@@ -120,18 +132,50 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 		float baseline,
 		float cx,
 		float cy,
-		const Transform & transform,
+		Transform localTransform,
 		int flowWinSize,
 		int flowMaxLevel,
 		int flowIterations,
-		double flowEps)
+		double flowEps,
+		double maxCorrespondencesSlope)
+{
+	std::vector<cv::Point2f> leftCorners;
+	cv::KeyPoint::convert(keypoints, leftCorners);
+	return generateKeypoints3DStereo(
+			leftCorners,
+			leftImage,
+			rightImage,
+			fx,
+			baseline,
+			cx,
+			cy,
+			localTransform,
+			flowWinSize,
+			flowMaxLevel,
+			flowIterations,
+			flowEps,
+			maxCorrespondencesSlope);
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr 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,
+		int flowWinSize,
+		int flowMaxLevel,
+		int flowIterations,
+		double flowEps,
+		double maxCorrespondencesSlope)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty() &&
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
 			leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
-
-	std::vector<cv::Point2f> leftCorners;
-	cv::KeyPoint::convert(keypoints, leftCorners);
+	UASSERT(fx > 0.0f && baseline > 0.0f);
 
 	// Find features in the new left image
 	std::vector<unsigned char> status;
@@ -151,28 +195,34 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 	UDEBUG("cv::calcOpticalFlowPyrLK() end");
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
-	keypoints3d->resize(keypoints.size());
+	keypoints3d->resize(leftCorners.size());
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
-	UASSERT(status.size() == keypoints.size());
+	UASSERT(status.size() == leftCorners.size());
 	for(unsigned int i=0; i<status.size(); ++i)
 	{
 		pcl::PointXYZ pt(bad_point, bad_point, bad_point);
 		if(status[i])
 		{
 			float disparity = leftCorners[i].x - rightCorners[i].x;
-			if(disparity > 0.0f)
+			float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
+			if(disparity > 0.0f &&
+			   (maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope))
 			{
 				pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
 						leftCorners[i],
 						disparity,
-						cx, cy, fx, baseline);
+						cx,
+						cy,
+						fx,
+						baseline);
 
 				if(pcl::isFinite(tmpPt))
 				{
 					pt = tmpPt;
-					if(!transform.isNull() && !transform.isIdentity())
+					if(!localTransform.isNull() &&
+					   !localTransform.isIdentity())
 					{
-						pt = util3d::transformPoint(pt, transform);
+						pt = util3d::transformPoint(pt, localTransform);
 					}
 				}
 			}
@@ -190,11 +240,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
 std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, cv::KeyPoint> & refWords,
 		const std::multimap<int, cv::KeyPoint> & nextWords,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		const Transform & localTransform,
+		const CameraModel & cameraModel,
 		Transform & cameraTransform,
 		int pnpIterations,
 		float pnpReprojError,
@@ -204,6 +250,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 		const std::multimap<int, pcl::PointXYZ> & refGuess3D,
 		double * varianceOut)
 {
+	UASSERT(cameraModel.isValid());
 	std::multimap<int, pcl::PointXYZ> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 	if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
@@ -257,10 +304,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					xp.at<double>(2, i) = 1;
 				}
 
-				cv::Mat K = (cv::Mat_<double>(3,3) <<
-					fx, 0, cx,
-					0, fy, cy,
-					0, 0, 1);
+				cv::Mat K = cameraModel.K();
 				cv::Mat Kinv = K.inv();
 				cv::Mat E = K.t()*F*K;
 				cv::Mat x_norm = Kinv * x;
@@ -280,7 +324,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 					//if camera transform is set, use it instead of the computed one from epipolar geometry
 					if(useCameraTransformGuess)
 					{
-						Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
+						Transform t = (cameraModel.localTransform().inverse()*cameraTransform*cameraModel.localTransform()).inverse();
 						P = (cv::Mat_<double>(3,4) <<
 								(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
 								(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
@@ -303,22 +347,10 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 						pts4D.col(i) /= pts4D.at<double>(3,i);
 						if(pts4D.at<double>(2,i) > 0)
 						{
-							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
+							words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
 						}
 					}
 
-					if(!useCameraTransformGuess)
-					{
-						cv::Mat R, T;
-						EpipolarGeometry::findRTFromP(P, R, T);
-
-						Transform t(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));
-
-						cameraTransform = (localTransform * t).inverse() * localTransform;
-					}
-
 					if(refGuess3D.size())
 					{
 						// scale estimation
@@ -408,7 +440,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 								imagePoints.resize(oi);
 
 								//PnPRansac
-								Transform guess = localTransform.inverse();
+								Transform guess = cameraModel.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(),
@@ -427,7 +459,11 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 										true,
 										pnpIterations,
 										pnpReprojError,
-										0,
+#if CV_MAJOR_VERSION < 3
+										0, // min inliers
+#else
+										0.99, // confidence
+#endif
 										inliersV,
 										pnpFlags);
 
@@ -440,12 +476,11 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 												   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));
 
-									cameraTransform = (localTransform * pnp).inverse();
+									cameraTransform = (cameraModel.localTransform() * pnp).inverse();
 								}
 								else
 								{
 									UWARN("No inliers after PnP!");
-									cameraTransform = Transform();
 								}
 							}
 						}
@@ -454,6 +489,17 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
 							UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
 						}
 					}
+					else if(!useCameraTransformGuess)
+					{
+						cv::Mat R, T;
+						EpipolarGeometry::findRTFromP(P, R, T);
+
+						Transform t(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));
+
+						cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
+					}
 				}
 			}
 		}
@@ -477,32 +523,6 @@ std::multimap<int, cv::KeyPoint> aggregate(
 	return words;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
-		const cv::Mat & image,
-		const cv::Mat & imageDepth,
-		float constant,
-		int fastThreshold,
-		bool fastNonmaxSuppression,
-		float maxDepth)
-{
-	// Extract words
-	cv::FastFeatureDetector detector(fastThreshold, fastNonmaxSuppression);
-	std::vector<cv::KeyPoint> kpts;
-	detector.detect(image, kpts);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
-	for(unsigned int i=0; i<kpts.size(); ++i)
-	{
-		pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
-		if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
-		{
-			points->push_back(pt);
-		}
-	}
-	UDEBUG("points %d -> %d", (int)kpts.size(), (int)points->size());
-	return points;
-}
-
 }
 
 }

corelib/src/util3d_filtering.cpp

@@ -52,10 +52,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(voxelSize > 0.0f);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::VoxelGrid<pcl::PointXYZ> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
@@ -66,10 +64,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(voxelSize > 0.0f);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
@@ -81,10 +77,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(samples > 0);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::RandomSample<pcl::PointXYZ> filter;
 	filter.setSample(samples);
 	filter.setInputCloud(cloud);
@@ -94,10 +88,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(samples > 0);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::RandomSample<pcl::PointXYZRGB> filter;
 	filter.setSample(samples);
 	filter.setInputCloud(cloud);
@@ -112,12 +104,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		float min,
 		float max)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> 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::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PassThrough<pcl::PointXYZ> filter;
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
@@ -132,12 +122,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 		float min,
 		float max)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> 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::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::PassThrough<pcl::PointXYZRGB> filter;
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
@@ -208,9 +196,7 @@ pcl::IndicesPtr radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius)
 {
-	typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-	KdTreePtr tree (new KdTree(false));
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>(false));
 
 	if(indices->size())
 	{
@@ -255,9 +241,7 @@ pcl::IndicesPtr radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius)
 {
-	typedef pcl::search::KdTree<pcl::PointXYZRGB> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-	KdTreePtr tree (new KdTree(false));
+	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
 
 	if(indices->size())
 	{
@@ -297,6 +281,82 @@ pcl::IndicesPtr radiusFiltering(
 	}
 }
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::copyPointCloud(*cloud, *indicesOut, *out);
+	return out;
+}
+
+
+pcl::IndicesPtr 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)
+{
+	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		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
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		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;
+	}
+}
+
 
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -583,7 +643,7 @@ pcl::IndicesPtr extractNegativeIndices(
 	return output;
 }
 pcl::IndicesPtr extractNegativeIndices(
-		const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices)
 {
 	pcl::IndicesPtr output(new std::vector<int>);

corelib/src/util3d_mapping.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_mapping.h"
 
-#include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d.h>

corelib/src/util3d_motion_estimation.cpp

@@ -0,0 +1,239 @@
+/*
+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/util3d_motion_estimation.h"
+
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_correspondences.h"
+
+namespace rtabmap
+{
+
+namespace util3d
+{
+
+Transform estimateMotion3DTo2D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
+			const CameraModel & cameraModel,
+			int minInliers,
+			int iterations,
+			double reprojError,
+			int flagsPnP,
+			const Transform & guess,
+			const std::map<int, pcl::PointXYZ> & words3B,
+			double * varianceOut,
+			std::vector<int> * matchesOut,
+			std::vector<int> * inliersOut)
+{
+
+	Transform transform;
+	std::vector<int> matches, inliers;
+
+	if(varianceOut)
+	{
+		*varianceOut = 1.0;
+	}
+
+	// find correspondences
+	std::vector<int> ids = uKeys(words2B);
+	std::vector<cv::Point3f> objectPoints(ids.size());
+	std::vector<cv::Point2f> imagePoints(ids.size());
+	int oi=0;
+	matches.resize(ids.size());
+	for(unsigned int i=0; i<ids.size(); ++i)
+	{
+		if(words3A.find(ids[i]) != words3A.end())
+		{
+			pcl::PointXYZ pt = words3A.find(ids[i])->second;
+			objectPoints[oi].x = pt.x;
+			objectPoints[oi].y = pt.y;
+			objectPoints[oi].z = pt.z;
+			imagePoints[oi] = words2B.find(ids[i])->second.pt;
+			matches[oi++] = ids[i];
+		}
+	}
+
+	objectPoints.resize(oi);
+	imagePoints.resize(oi);
+	matches.resize(oi);
+
+	if((int)matches.size() >= minInliers)
+	{
+		//PnPRansac
+		cv::Mat K = cameraModel.K();
+		Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
+		cv::Mat R = (cv::Mat_<double>(3,3) <<
+				(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
+				(double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
+				(double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());
+
+		cv::Mat rvec(1,3, CV_64FC1);
+		cv::Rodrigues(R, rvec);
+		cv::Mat tvec = (cv::Mat_<double>(1,3) <<
+				(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
+
+		cv::solvePnPRansac(
+				objectPoints,
+				imagePoints,
+				K,
+				cv::Mat(),
+				rvec,
+				tvec,
+				true,
+				iterations,
+				reprojError,
+#if CV_MAJOR_VERSION < 3
+				0, // min inliers
+#else
+				0.99, // confidence
+#endif
+				inliers,
+				flagsPnP);
+
+		if((int)inliers.size() >= minInliers)
+		{
+			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));
+
+			transform = (cameraModel.localTransform() * pnp).inverse();
+
+			// compute variance (like in PCL computeVariance() method of sac_model.h)
+			if(varianceOut && words3B.size())
+			{
+				std::vector<float> errorSqrdDists(inliers.size());
+				oi = 0;
+				for(unsigned int i=0; i<inliers.size(); ++i)
+				{
+					std::multimap<int, pcl::PointXYZ>::const_iterator iter = words3B.find(matches[inliers[i]]);
+					if(iter != words3B.end() && pcl::isFinite(iter->second))
+					{
+						const cv::Point3f & objPt = objectPoints[inliers[i]];
+						pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
+						errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+					}
+				}
+				errorSqrdDists.resize(oi);
+				if(errorSqrdDists.size())
+				{
+					std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+					double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+					*varianceOut = 2.1981 * median_error_sqr;
+				}
+			}
+		}
+	}
+
+	if(matchesOut)
+	{
+		*matchesOut = matches;
+	}
+	if(inliersOut)
+	{
+		inliersOut->resize(inliers.size());
+		for(unsigned int i=0; i<inliers.size(); ++i)
+		{
+			inliersOut->at(i) = matches[inliers[i]];
+		}
+	}
+
+	return transform;
+}
+
+Transform estimateMotion3DTo3D(
+			const std::map<int, pcl::PointXYZ> & words3A,
+			const std::map<int, pcl::PointXYZ> & words3B,
+			int minInliers,
+			double inliersDistance,
+			int iterations,
+			int refineIterations,
+			double * varianceOut,
+			std::vector<int> * matchesOut,
+			std::vector<int> * inliersOut)
+{
+	Transform transform;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
+	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
+
+	std::vector<int> matches;
+	util3d::findCorrespondences(
+			words3A,
+			words3B,
+			*inliers1,
+			*inliers2,
+			0,
+			&matches);
+
+	if(varianceOut)
+	{
+		*varianceOut = 1.0;
+	}
+
+	if((int)inliers1->size() >= minInliers)
+	{
+		std::vector<int> inliers;
+		Transform t = util3d::transformFromXYZCorrespondences(
+				inliers2,
+				inliers1,
+				inliersDistance,
+				iterations,
+				refineIterations>0,
+				3.0,
+				refineIterations,
+				&inliers,
+				varianceOut);
+
+		if(!t.isNull() && (int)inliers.size() >= minInliers)
+		{
+			transform = t;
+		}
+
+		if(matchesOut)
+		{
+			*matchesOut = matches;
+		}
+
+		if(inliersOut)
+		{
+			inliersOut->resize(inliers.size());
+			for(unsigned int i=0; i<inliers.size(); ++i)
+			{
+				inliersOut->at(i) = matches[inliers[i]];
+			}
+		}
+	}
+	return transform;
+}
+
+}
+
+}

corelib/src/util3d_registration.cpp

@@ -222,14 +222,79 @@ Transform transformFromXYZCorrespondences(
 	return Transform();
 }
 
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	variance = 1;
+	correspondencesOut = 0;
+	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
+	est->setInputTarget(cloudA);
+	est->setInputSource(cloudB);
+	pcl::Correspondences correspondences;
+	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+
+	if(correspondences.size()>=3)
+	{
+		std::vector<double> distances(correspondences.size());
+		for(unsigned int i=0; i<correspondences.size(); ++i)
+		{
+			distances[i] = correspondences[i].distance;
+		}
+
+		//variance
+		std::sort(distances.begin (), distances.end ());
+		double median_error_sqr = distances[distances.size () >> 1];
+		variance = (2.1981 * median_error_sqr);
+	}
+
+	correspondencesOut = (int)correspondences.size();
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	variance = 1;
+	correspondencesOut = 0;
+	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+	est->setInputTarget(cloudA);
+	est->setInputSource(cloudB);
+	pcl::Correspondences correspondences;
+	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
+
+	if(correspondences.size()>=3)
+	{
+		std::vector<double> distances(correspondences.size());
+		for(unsigned int i=0; i<correspondences.size(); ++i)
+		{
+			distances[i] = correspondences[i].distance;
+		}
+
+		//variance
+		std::sort(distances.begin (), distances.end ());
+		double median_error_sqr = distances[distances.size () >> 1];
+		variance = (2.1981 * median_error_sqr);
+	}
+
+	correspondencesOut = (int)correspondences.size();
+}
+
 // return transform from source to target (All points must be finite!!!)
 Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool * hasConvergedOut,
-			  double * variance,
-			  int * correspondencesOut)
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -247,63 +312,8 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
-	icp.align (*cloud_source_registered);
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
+	icp.align (cloud_source_registered);
+	hasConverged = icp.hasConverged();
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -313,9 +323,8 @@ Transform icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
-		bool * hasConvergedOut,
-		double * variance,
-		int * correspondencesOut)
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered)
 {
 	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
 	// Set the input source and target
@@ -337,63 +346,8 @@ Transform icpPointToPlane(
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
-	icp.align (*cloud_source_registered);
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
+	icp.align (cloud_source_registered);
+	hasConverged = icp.hasConverged();
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
@@ -402,9 +356,8 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
-			  bool * hasConvergedOut,
-			  double * variance,
-			  int * correspondencesOut)
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
@@ -426,63 +379,8 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
 
 	// Perform the alignment
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
-	icp.align (*cloud_source_registered);
-	bool hasConverged = icp.hasConverged();
-
-	// compute variance
-	if((correspondencesOut || variance) && hasConverged)
-	{
-		pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-		est->setInputTarget(cloud_target);
-		est->setInputSource(cloud_source_registered);
-		pcl::Correspondences correspondences;
-		est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
-		if(variance)
-		{
-			if(correspondences.size()>=3)
-			{
-				std::vector<double> distances(correspondences.size());
-				for(unsigned int i=0; i<correspondences.size(); ++i)
-				{
-					distances[i] = correspondences[i].distance;
-				}
-
-				//variance
-				std::sort(distances.begin (), distances.end ());
-				double median_error_sqr = distances[distances.size () >> 1];
-				*variance = (2.1981 * median_error_sqr);
-			}
-			else
-			{
-				hasConverged = false;
-				*variance = -1.0;
-			}
-		}
-
-		if(correspondencesOut)
-		{
-			*correspondencesOut = (int)correspondences.size();
-		}
-	}
-	else
-	{
-		if(correspondencesOut)
-		{
-			*correspondencesOut = 0;
-		}
-		if(variance)
-		{
-			*variance = -1;
-		}
-	}
-
-	if(hasConvergedOut)
-	{
-		*hasConvergedOut = hasConverged;
-	}
-
+	icp.align (cloud_source_registered);
+	hasConverged = icp.hasConverged();
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 

corelib/src/util3d_transforms.cpp

@@ -36,22 +36,18 @@ namespace util3d
 {
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
 	return output;
 }

examples/BOWMapping/main.cpp

@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/Rtabmap.h"
-#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraRGB.h"
 #include <opencv2/core/core.hpp>
 #include "rtabmap/utilite/UFile.h"
 #include <stdio.h>
@@ -118,12 +118,12 @@ int main(int argc, char * argv[])
 
 	int countLoopDetected=0;
 	int i=0;
-	cv::Mat img = camera.takeImage();
+	rtabmap::SensorData data = camera.takeImage();
 	int nextIndex = rtabmap.getLastLocationId()+1;
-	while(!img.empty())
+	while(!data.imageRaw().empty())
 	{
 		// Process image : Main loop of RTAB-Map
-		rtabmap.process(img, nextIndex);
+		rtabmap.process(data.imageRaw(), nextIndex);
 
 		// Check if a loop closure is detected and print some info
 		if(rtabmap.getLoopClosureId())
@@ -157,7 +157,7 @@ int main(int argc, char * argv[])
 		++nextIndex;
 
 		//Get next image
-		img = camera.takeImage();
+		data = camera.takeImage();
 	}
 
 	printf("Processing images completed. Loop closures found = %d\n", countLoopDetected);

examples/RGBDMapping/MapBuilder.h

@@ -71,8 +71,8 @@ public:
 		layout->addWidget(cloudViewer_);
 		this->setLayout(layout);
 
+		qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
-		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 
 		QAction * pause = new QAction(this);
 		this->addAction(pause);
@@ -102,14 +102,14 @@ protected slots:
 		}
 	}
 
-	virtual void processOdometry(const rtabmap::SensorData & data)
+	virtual void processOdometry(const rtabmap::OdometryEvent & odom)
 	{
 		if(!this->isVisible())
 		{
 			return;
 		}
 
-		Transform pose = data.pose();
+		Transform pose = odom.pose();
 		if(pose.isNull())
 		{
 			//Odometry lost
@@ -126,38 +126,33 @@ protected slots:
 			lastOdomPose_ = pose;
 
 			// 3d cloud
-			if(data.depth().cols == data.image().cols &&
-			   data.depth().rows == data.image().rows &&
-			   !data.depth().empty() &&
-			   data.fx() > 0.0f &&
-			   data.fy() > 0.0f)
+			if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
+			   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
+			   !odom.data().depthOrRightRaw().empty() &&
+			   (odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
-					data.image(),
-					data.depth(),
-					data.cx(),
-					data.cy(),
-					data.fx(),
-					data.fy(),
-					2); // decimation // high definition
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+					odom.data(),
+					2,     // decimation
+					4.0f); // max depth
 				if(cloud->size())
 				{
-					cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
-					if(cloud->size())
+					if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
 					{
-						cloud = util3d::transformPointCloud(cloud, data.localTransform());
+						UERROR("Adding cloudOdom to viewer failed!");
 					}
 				}
-				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
+				else
 				{
-					UERROR("Adding cloudOdom to viewer failed!");
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					UWARN("Empty cloudOdom!");
 				}
 			}
 
-			if(!data.pose().isNull())
+			if(!odom.pose().isNull())
 			{
 				// update camera position
-				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*data.pose());
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
 			}
 		}
 		cloudViewer_->update();
@@ -196,35 +191,32 @@ protected slots:
 					}
 					cloudViewer_->setCloudVisibility(cloudName, true);
 				}
-				else if(iter->first == stats.refImageId() &&
-						stats.getSignature().id() == iter->first)
+				else if(uContains(stats.getSignatures(), iter->first))
 				{
-					Signature s = stats.getSignature();
-					s.uncompressData(); // make sure data is uncompressed
+					Signature s = stats.getSignatures().at(iter->first);
+					s.sensorData().uncompressData(); // make sure data is uncompressed
 					// Add the new cloud
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
-							s.getImageRaw(),
-							s.getDepthRaw(),
-							s.getCx(),
-							s.getCy(),
-							s.getFx(),
-							s.getFy(),
-						   4); // decimation
-
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+						    4,     // decimation
+						    4.0f); // max depth
 					if(cloud->size())
 					{
-						cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
-						if(cloud->size())
+						if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
 						{
-							cloud = util3d::transformPointCloud(cloud, stats.getSignature().getLocalTransform());
+							UERROR("Adding cloud %d to viewer failed!", iter->first);
 						}
 					}
-					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
+					else
 					{
-						UERROR("Adding cloud %d to viewer failed!", iter->first);
+						UWARN("Empty cloud %d!", iter->first);
 					}
 				}
 			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
 		}
 
 		//============================
@@ -278,7 +270,7 @@ protected slots:
 			   !processingStatistics_)
 			{
 				lastOdometryProcessed_ = false; // if we receive too many odometry events!
-				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
 			}
 		}
 	}

examples/RGBDMapping/main.cpp

@@ -71,7 +71,7 @@ int main(int argc, char * argv[])
 
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = 0;
+	Camera * camera = 0;
 	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 1)
 	{
@@ -114,13 +114,14 @@ int main(int argc, char * argv[])
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
 	}
 
-	CameraThread cameraThread(camera);
-	if(!cameraThread.init())
+	if(!camera->init())
 	{
 		UERROR("Camera init failed!");
-		exit(1);
 	}
 
+	CameraThread cameraThread(camera);
+
+
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
 	// We give it the camera so the GUI can pause/resume the camera
 	QApplication app(argc, argv);

examples/WifiMapping/MapBuilderWifi.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAPBUILDERWIFI_H_
 
 #include "../RGBDMapping/MapBuilder.h"
+#include "rtabmap/core/UserDataEvent.h"
 
 using namespace rtabmap;
 
@@ -64,6 +65,28 @@ public:
 		this->unregisterFromEventsManager();
 	}
 
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("UserDataEvent") == 0)
+		{
+			UserDataEvent * rtabmapEvent = (UserDataEvent *)event;
+			// convert userData to wifi levels
+			if(!rtabmapEvent->data().empty())
+			{
+				UASSERT(rtabmapEvent->data().type() == CV_64FC1 &&
+						rtabmapEvent->data().cols == 2 &&
+						rtabmapEvent->data().rows == 1);
+
+				// format [int level, double stamp]
+				int level = rtabmapEvent->data().at<double>(0);
+				double stamp = rtabmapEvent->data().at<double>(1);
+				wifiLevels_.insert(std::make_pair(stamp, level));
+			}
+		}
+		MapBuilder::handleEvent(event);
+	}
+
 protected slots:
 	virtual void processStatistics(const rtabmap::Statistics & stats)
 	{
@@ -76,43 +99,29 @@ protected slots:
 		// Add WIFI symbols
 		//============================
 		std::map<double, int> nodeStamps; // <stamp, id>
-		std::map<int, std::pair<int, double> > wifiLevels;
 
-		UASSERT(stats.getStamps().size() == stats.getUserDatas().size());
-		std::map<int, double>::const_iterator iterStamps = stats.getStamps().begin();
-		std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = stats.getUserDatas().begin();
-		for(; iterStamps!=stats.getStamps().end() && iterUserDatas!=stats.getUserDatas().end(); ++iterStamps, ++iterUserDatas)
+		for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
+			iter!=stats.getSignatures().end();
+			++iter)
 		{
-			// Sort stamps by stamps
-			nodeStamps.insert(std::make_pair(iterStamps->second, iterStamps->first));
-
-			// convert userData to wifi levels
-			if(iterUserDatas->second.size())
-			{
-				UASSERT(iterUserDatas->second.size() == sizeof(int)+sizeof(double));
-
-				// format [int level, double stamp]
-				int level;
-				double stamp;
-				memcpy(&level, iterUserDatas->second.data(), sizeof(int));
-				memcpy(&stamp, iterUserDatas->second.data()+sizeof(int), sizeof(double));
-
-				wifiLevels.insert(std::make_pair(iterUserDatas->first, std::make_pair(level, stamp)));
-			}
+			// Sort stamps by stamps->id
+			nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
 		}
 
-		for(std::map<int, std::pair<int, double> >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter)
+		int id = 0;
+		for(std::map<double, int>::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id)
 		{
 			// The Wifi value may be taken between two nodes, interpolate its position.
-			double stampWifi = iter->second.second;
+			double stampWifi = iter->first;
 			std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
 			if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
 			{
 				--previousNode;
 			}
-			std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(iter->second.second); // upper bound of the stamp
+			std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
 
-			if(previousNode != nodeStamps.end() && nextNode != nodeStamps.end() &&
+			if(previousNode != nodeStamps.end() &&
+			   nextNode != nodeStamps.end() &&
 			   previousNode->second != nextNode->second &&
 			   uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
 			{
@@ -131,18 +140,18 @@ protected slots:
 
 				Transform wifiPose = (poseA*v).translation(); // rip off the rotation
 
-				std::string cloudName = uFormat("level%d", iter->first);
+				std::string cloudName = uFormat("level%d", id);
 				if(clouds.contains(cloudName))
 				{
 					if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
 					{
-						UERROR("Updating pose cloud %d failed!", iter->first);
+						UERROR("Updating pose cloud %d failed!", id);
 					}
 				}
 				else
 				{
 					// Make a line with points
-					int quality = dBm2Quality(iter->second.first)/10;
+					int quality = dBm2Quality(iter->second)/10;
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					for(int i=0; i<10; ++i)
 					{
@@ -168,7 +177,7 @@ protected slots:
 					//UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size());
 					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, wifiPose, Qt::yellow))
 					{
-						UERROR("Adding cloud %d to viewer failed!", iter->first);
+						UERROR("Adding cloud %d to viewer failed!", id);
 					}
 					else
 					{
@@ -176,10 +185,6 @@ protected slots:
 					}
 				}
 			}
-			else
-			{
-				UWARN("Bounds not found!");
-			}
 		}
 
 		//============================
@@ -187,6 +192,9 @@ protected slots:
 		//============================
 		MapBuilder::processStatistics(stats);
 	}
+
+private:
+	std::map<double, int> wifiLevels_;
 };
 
 

examples/WifiMapping/WifiThread.h

@@ -207,10 +207,10 @@ private:
 			{
 				double stamp = UTimer::now();
 
-				// Create user data [level, stamp] with the value (int = 4 bytes) and a timestamp (double = 8 bytes)
-				std::vector<unsigned char> data(sizeof(int) + sizeof(double));
-				memcpy(data.data(), &dBm, sizeof(int));
-				memcpy(data.data()+sizeof(int), &stamp, sizeof(double));
+				// Create user data [level, stamp] with the value and a timestamp
+				cv::Mat data(1, 2, CV_64FC1);
+				data.at<double>(0) = double(dBm);
+				data.at<double>(1) = stamp;
 				this->post(new UserDataEvent(data));
 				//UWARN("posting level %d dBm", dBm);
 			}

examples/WifiMapping/main.cpp

@@ -56,7 +56,7 @@ int main(int argc, char * argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
-	std::string interfaceName = "eth0";
+	std::string interfaceName = "wlan0";
 	int driver = 0;
 	bool mirroring = false;
 
@@ -109,7 +109,7 @@ int main(int argc, char * argv[])
 
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = 0;
+	Camera * camera = 0;
 	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	if(driver == 1)
 	{
@@ -152,16 +152,17 @@ int main(int argc, char * argv[])
 		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
 	}
 
+
+	if(!camera->init())
+	{
+		UERROR("Camera init failed! Try another camera driver.");
+		showUsage();
+		exit(1);
+	}
+	CameraThread cameraThread(camera);
 	if(mirroring)
 	{
-		camera->setMirroringEnabled(true);
-	}
-
-	CameraThread cameraThread(camera);
-	if(!cameraThread.init())
-	{
-		UERROR("Camera init failed!");
-		//exit(1);
+		cameraThread.setMirroringEnabled(true);
 	}
 
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map

guilib/include/rtabmap/gui/CloudViewer.h

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QSettings>
 
 #include <opencv2/opencv.hpp>
+#include <set>
 
 #include <pcl/visualization/mouse_event.h>
 #include <pcl/PCLPointCloud2.h>
@@ -136,6 +137,13 @@ public:
 	void updateCameraTargetPosition(
 		const Transform & pose);
 
+	void addOrUpdateCoordinate(
+			const std::string & id,
+			const Transform & transform,
+			double scale);
+	void removeCoordinate(const std::string & id);
+	void removeAllCoordinates();
+
 	void addOrUpdateGraph(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
@@ -143,6 +151,15 @@ public:
 	void removeGraph(const std::string & id);
 	void removeAllGraphs();
 
+	void addOrUpdateText(
+			const std::string & id,
+			const std::string & text,
+			const Transform & position,
+			double scale,
+			const QColor & color);
+	void removeText(const std::string & id);
+	void removeAllTexts();
+
 	bool isTrajectoryShown() const;
 	unsigned int getTrajectorySize() const;
 	void setTrajectoryShown(bool shown);
@@ -190,7 +207,7 @@ public slots:
 	void setCloudVisibility(const std::string & id, bool isVisible);
 	void setCloudOpacity(const std::string & id, double opacity = 1.0);
 	void setCloudPointSize(const std::string & id, int size);
-	virtual void clear() {removeAllClouds(); clearTrajectory();}
+	virtual void clear();
 
 signals:
 	void configChanged();
@@ -200,13 +217,13 @@ protected:
 	virtual void keyPressEvent(QKeyEvent * event);
 	virtual void mousePressEvent(QMouseEvent * event);
 	virtual void mouseMoveEvent(QMouseEvent * event);
+	virtual void wheelEvent(QWheelEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 	virtual void handleAction(QAction * event);
 	QMenu * menu() {return _menu;}
 
 private:
 	void createMenu();
-	void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void);
 	void addGrid();
 	void removeGrid();
 
@@ -225,10 +242,14 @@ private:
     QAction * _aSetBackgroundColor;
     QMenu * _menu;
     std::set<std::string> _graphes;
+    std::set<std::string> _coordinates;
+    std::set<std::string> _texts;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
     unsigned int _gridCellCount;
     float _gridCellSize;
+    cv::Vec3d _lastCameraOrientation;
+    cv::Vec3d _lastCameraPose;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;

guilib/include/rtabmap/gui/DataRecorder.h

@@ -57,7 +57,7 @@ public:
 	const QString & path() const {return path_;}
 
 public slots:
-	void addData(const rtabmap::SensorData & data);
+	void addData(const rtabmap::SensorData & data, const Transform & pose = Transform(), const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1));
 	void showImage(const cv::Mat & image, const cv::Mat & depth);
 protected:
 	virtual void closeEvent(QCloseEvent* event);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -52,7 +52,7 @@ namespace rtabmap
 {
 class Memory;
 class ImageView;
-class Signature;
+class SensorData;
 class CloudViewer;
 
 class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
@@ -105,6 +105,7 @@ private slots:
 	void resetConstraint();
 	void rejectConstraint();
 	void updateConstraintView();
+	void updateStereo();
 
 private:
 	QString getIniFilePath() const;
@@ -122,8 +123,9 @@ private:
 				rtabmap::CloudViewer * view3D,
 				QLabel * labelId,
 				QLabel * labelMapId,
+				QLabel * labelPose,
 				bool updateConstraintView);
-	void updateStereo(const Signature * data);
+	void updateStereo(const SensorData * data);
 	void updateWordsMatching();
 	void updateConstraintView(
 			const rtabmap::Link & link,
@@ -148,12 +150,15 @@ private:
 private:
 	Ui_DatabaseViewer * ui_;
 	QList<int> ids_;
+	std::map<int, int> mapIds_;
 	QMap<int, int> idToIndex_;
 	QList<rtabmap::Link> neighborLinks_;
 	QList<rtabmap::Link> loopLinks_;
 	rtabmap::Memory * memory_;
 	QString pathDatabase_;
+	std::string databaseFileName_;
 	std::list<std::map<int, rtabmap::Transform> > graphes_;
+	std::multimap<int, rtabmap::Link> graphLinks_;
 	std::map<int, rtabmap::Transform> poses_;
 	std::multimap<int, rtabmap::Link> links_;
 	std::multimap<int, rtabmap::Link> linksRefined_;

guilib/include/rtabmap/gui/ImageView.h

@@ -55,6 +55,7 @@ public:
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
 
+	QRectF sceneRect() const;
 	bool isImageShown() const;
 	bool isImageDepthShown() const;
 	bool isFeaturesShown() const;

guilib/include/rtabmap/gui/MainWindow.h

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/gui/PreferencesDialog.h"
 
 #include <pcl/point_cloud.h>
@@ -86,13 +86,6 @@ public:
 		kMonitoringPaused
 	};
 
-	enum SrcType {
-		kSrcUndefined,
-		kSrcVideo,
-		kSrcImages,
-		kSrcStream
-	};
-
 public:
 	/**
 	 * @param prefDialog If NULL, a default dialog is created. This
@@ -131,18 +124,17 @@ private slots:
 	void startDetection();
 	void pauseDetection();
 	void stopDetection();
+	void notifyNoMoreImages();
 	void printLoopClosureIds();
-	void generateMap();
-	void generateLocalMap();
-	void generateTOROMap();
+	void generateGraphDOT();
+	void exportPosesKITTI();
+	void exportPosesRGBDSLAM();
+	void exportPosesTORO();
 	void postProcessing();
 	void deleteMemory();
 	void openWorkingDirectory();
 	void updateEditMenu();
-	void selectImages();
-	void selectVideo();
 	void selectStream();
-	void selectDatabase();
 	void selectOpenni();
 	void selectFreenect();
 	void selectOpenniCv();
@@ -154,24 +146,28 @@ private slots:
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
+	void cancelGoal();
+	void label();
 	void downloadAllClouds();
 	void downloadPoseGraph();
 	void clearTheCache();
 	void openPreferences();
+	void openPreferencesSource();
+	void setDefaultViews();
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
 	void updateElapsedTime();
-	void processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
+	void processOdometry(const rtabmap::OdometryEvent & odom);
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
 	void processRtabmapEventInit(int status, const QString & info);
 	void processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & event);
 	void processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event);
+	void processRtabmapLabelErrorEvent(int id, const QString & label);
 	void changeImgRateSetting();
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
 	void changeMappingMode();
-	void captureScreen();
 	void setAspectRatio(int w, int h);
 	void setAspectRatio16_9();
 	void setAspectRatio16_10();
@@ -194,12 +190,13 @@ private slots:
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
-	void odometryReceived(const rtabmap::SensorData &, const rtabmap::OdometryInfo &);
+	void odometryReceived(const rtabmap::OdometryEvent &);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);
 	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
 	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
+	void rtabmapLabelErrorReceived(int id, const QString & label);
 	void imgRateChanged(double);
 	void detectionRateChanged(double);
 	void timeLimitChanged(float);
@@ -210,8 +207,14 @@ signals:
 
 private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
-	void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints, const std::map<int, int> & mapIds, bool verboseProgress = false);
-	void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
+	void updateMapCloud(
+			const std::map<int, Transform> & poses,
+			const Transform & pose,
+			const std::multimap<int, Link> & constraints,
+			const std::map<int, int> & mapIds,
+			const std::map<int, std::string> & labels,
+			bool verboseProgress = false);
+	void createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
@@ -219,6 +222,8 @@ private:
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
 	void saveFigures();
 	void loadFigures();
+	void exportPoses(int format);
+	QString captureScreen();
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
 			const std::map<int, Transform> & poses,
@@ -227,19 +232,6 @@ private:
 			int regenerateDecimation,
 			float regenerateVoxelSize,
 			float regenerateMaxDepth) const;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
-			int id,
-			const cv::Mat & rgb,
-			const cv::Mat & depth,
-			float fx,
-			float fy,
-			float cx,
-			float cy,
-			const Transform & localTransform,
-			const Transform & pose,
-			float voxelSize,
-			int decimation,
-			float maxDepth) const;
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
 			const std::map<int, Transform> & poses,
 			bool regenerateClouds,
@@ -261,9 +253,6 @@ private:
 	rtabmap::DBReader * _dbReader;
 	rtabmap::OdometryThread * _odomThread;
 
-	SrcType _srcType;
-	QString _srcPath;
-
 	//Dialogs
 	PreferencesDialog * _preferencesDialog;
 	AboutDialog * _aboutDialog;
@@ -274,6 +263,7 @@ private:
 	QSet<int> _lastIds;
 	int _lastId;
 	bool _processingStatistics;
+	bool _processingDownloadedMap;
 	bool _odometryReceived;
 	QString _newDatabasePath;
 	QString _newDatabasePathOutput;
@@ -287,6 +277,7 @@ private:
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
 	std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
 	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
+	std::map<int, std::string> _curentLabels; // <nodeId, label>
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
@@ -307,7 +298,7 @@ private:
 	DetailedProgressDialog * _initProgressDialog;
 
 	QString _graphSavingFileName;
-	QString _toroSavingFileName;
+	QMap<int, QString> _exportPosesFileName;
 	bool _autoScreenCaptureOdomSync;
 
 	QVector<int> _refIds;

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -30,8 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
 #include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"
 
@@ -49,7 +48,7 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler
 	Q_OBJECT
 
 public:
-	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, int qualityWarningThr=0, QWidget * parent = 0);
+	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0);
 	virtual ~OdometryViewer();
 
 public slots:
@@ -59,7 +58,8 @@ protected:
 	virtual void handleEvent(UEvent * event);
 
 private slots:
-	void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
+	void reset();
+	void processData(const rtabmap::OdometryEvent & odom);
 
 private:
 	ImageView* imageView_;
@@ -76,6 +76,7 @@ private:
 	QSpinBox * maxCloudsSpin_;
 	QDoubleSpinBox * voxelSpin_;
 	QSpinBox * decimationSpin_;
+	QDoubleSpinBox * maxDepthSpin_;
 	QLabel * timeLabel_;
 	int validDecimationValue_;
 };

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -59,7 +59,7 @@ namespace rtabmap {
 
 class Signature;
 class LoopClosureViewer;
-class CameraRGBD;
+class Camera;
 class CalibrationDialog;
 
 class RTABMAPGUI_EXP PreferencesDialog : public QDialog
@@ -79,18 +79,29 @@ public:
 	Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag);
 
 	enum Src {
-		kSrcUndef,
-		kSrcUsbDevice,
-		kSrcImages,
-		kSrcVideo,
-		kSrcOpenNI_PCL,
-		kSrcFreenect,
-		kSrcOpenNI_CV,
-		kSrcOpenNI_CV_ASUS,
-		kSrcOpenNI2,
-		kSrcFreenect2,
-		kSrcStereoDC1394,
-		kSrcStereoFlyCapture2
+		kSrcUndef = -1,
+
+		kSrcRGBD           = 0,
+		kSrcOpenNI_PCL     = 0,
+		kSrcFreenect       = 1,
+		kSrcOpenNI_CV      = 2,
+		kSrcOpenNI_CV_ASUS = 3,
+		kSrcOpenNI2        = 4,
+		kSrcFreenect2      = 5,
+		kSrcRGBDImages     = 6,
+
+		kSrcStereo         = 100,
+		kSrcDC1394         = 100,
+		kSrcFlyCapture2    = 101,
+		kSrcStereoImages   = 102,
+		kSrcStereoVideo    = 103,
+
+		kSrcRGB            = 200,
+		kSrcUsbDevice      = 200,
+		kSrcImages         = 201,
+		kSrcVideo          = 202,
+
+		kSrcDatabase       = 300
 	};
 
 public:
@@ -99,6 +110,7 @@ public:
 
 	virtual QString getIniFilePath() const;
 	void init();
+	void setCurrentPanelToSource();
 
 	// save stuff
 	void saveSettings();
@@ -129,6 +141,7 @@ public:
 	bool isPosteriorGraphView() const;
 
 	bool isGraphsShown() const;
+	bool isLabelsShown() const;
 	bool isCloudMeshing() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	double getCloudVoxelSize(int index) const; // 0=map, 1=odom
@@ -147,8 +160,10 @@ public:
 	double getMeshSmoothingRadius() const;
 
 	bool isCloudFiltering() const;
+	bool isSubtractFiltering() const;
 	double getCloudFilteringRadius() const;
 	double getCloudFilteringAngle() const;
+	int getSubstractFilteringMinPts() const;
 
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;
@@ -161,36 +176,20 @@ public:
 	// source panel
 	double getGeneralInputRate() const;
 	bool isSourceMirroring() const;
-	bool isSourceImageUsed() const;
-	bool isSourceDatabaseUsed() const;
-	bool isSourceRGBDUsed() const;
-	PreferencesDialog::Src getSourceImageType() const;
-	QString getSourceImageTypeStr() const;
-	int getSourceWidth() const;
-	int getSourceHeight() const;
-	QString getSourceImagesPath() const;	//Images group
-	QString getSourceImagesSuffix() const;	//Images group
-	int getSourceImagesSuffixIndex() const;	//Images group
-	int getSourceImagesStartPos() const;	//Images group
-	bool getSourceImagesRefreshDir() const;	//Images group
-	QString getSourceVideoPath() const;	//Video group
-	int getSourceUsbDeviceId() const;		//UsbDevice group
+	QString getCalibrationName() const;
+	PreferencesDialog::Src getSourceType() const;
+	PreferencesDialog::Src getSourceDriver() const;
+	QString getSourceDriverStr() const;
+	QString getSourceDevice() const;
+
 	QString getSourceDatabasePath() const; //Database group
 	bool getSourceDatabaseOdometryIgnored() const; //Database group
 	bool getSourceDatabaseGoalDelayIgnored() const; //Database group
 	int getSourceDatabaseStartPos() const; //Database group
 	bool getSourceDatabaseStampsUsed() const;//Database group
-	Src getSourceRGBD() const; 			// Openni group
-	bool getSourceOpenni2AutoWhiteBalance() const;  //Openni group
-	bool getSourceOpenni2AutoExposure() const;  //Openni group
-	int getSourceOpenni2Exposure() const;  //Openni group
-	int getSourceOpenni2Gain() const;   //Openni group
-	bool getSourceOpenni2Mirroring() const; //Openni group
-	int getSourceFreenect2Format() const; //Openni group
 	bool isSourceRGBDColorOnly() const;
-	QString getSourceOpenniDevice() const;            //Openni group
-	Transform getSourceOpenniLocalTransform() const;    //Openni group
-	CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
+	Transform getSourceLocalTransform() const;    //Openni group
+	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 
@@ -205,6 +204,7 @@ public:
 	double getLoopThr() const;
 	double getVpThr() const;
 	int getOdomStrategy() const;
+	int getOdomBufferSize() const;
 	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//
@@ -219,10 +219,9 @@ public slots:
 	void setDetectionRate(double value);
 	void setTimeLimit(float value);
 	void setSLAMMode(bool enabled);
-	void selectSourceImage(Src src = kSrcUndef);
-	void selectSourceDatabase(bool user = false);
-	void selectSourceRGBD(Src src = kSrcUndef);
+	void selectSourceDriver(Src src);
 	void calibrate();
+	void calibrateSimple();
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -244,13 +243,29 @@ private slots:
 	void updateKpROI();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
+	void changeOdomBowFixedLocalMapPath();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
+	void selectSourceDatabase();
+	void selectSourceRGBDImagesStamps();
+	void selectSourceRGBDImagesPathRGB();
+	void selectSourceRGBDImagesPathDepth();
+	void selectSourceStereoImagesStamps();
+	void selectSourceStereoImagesPathLeft();
+	void selectSourceStereoImagesPathRight();
+	void selectSourceImagesPath();
+	void selectSourceVideoPath();
+	void selectSourceStereoVideoPath();
+	void selectSourceOniPath();
+	void selectSourceOni2Path();
+	void updateSourceGrpVisibility();
 	void updateRGBDCameraGroupBoxVisibility();
+	void updateRGBCameraGroupBoxVisibility();
+	void updateStereoCameraGroupBoxVisibility();
 	void testOdometry();
-	void testRGBDCamera();
+	void testCamera();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );

guilib/src/AboutDialog.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "AboutDialog.h"
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/Graph.h"
 #include "ui_aboutDialog.h"
 #include <opencv2/core/version.hpp>

guilib/src/CMakeLists.txt

@@ -24,6 +24,7 @@ SET(headers_ui
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
 	./GraphViewer.h
+	./CreateSimpleCalibrationDialog.h
 )
 
 SET(uis
@@ -37,6 +38,7 @@ SET(uis
     ./ui/postProcessingDialog.ui
     ./ui/exportCloudsDialog.ui
     ./ui/calibrationDialog.ui
+    ./ui/createSimpleCalibrationDialog.ui
 )
 
 SET(qrc 
@@ -83,6 +85,7 @@ SET(SRC_FILES
 	./ExportCloudsDialog.cpp
 	./MapVisibilityWidget.cpp
 	./GraphViewer.cpp
+	./CreateSimpleCalibrationDialog.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}

guilib/src/CalibrationDialog.cpp

@@ -167,6 +167,7 @@ void CalibrationDialog::setStereoMode(bool stereo)
 	ui_->lineEdit_R_2->setVisible(stereo_);
 	ui_->lineEdit_P_2->setVisible(stereo_);
 	ui_->radioButton_stereoRectified->setVisible(stereo_);
+	ui_->checkBox_switchImages->setVisible(stereo_);
 }
 
 void CalibrationDialog::setBoardWidth(int width)
@@ -198,7 +199,11 @@ void CalibrationDialog::setSquareSize(double size)
 
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
-	if(!savedCalibration_ && models_[0].isValid() && (!stereo_ || stereoModel_.isValid()))
+	if(!savedCalibration_ && models_[0].isValid() &&
+			(!stereo_ ||
+					(stereoModel_.left().isValid() &&
+					stereoModel_.right().isValid()&&
+					(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
 	{
 		QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
 				tr("The camera is calibrated but you didn't "
@@ -234,13 +239,12 @@ void CalibrationDialog::handleEvent(UEvent * event)
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
-			if(e->getCode() == rtabmap::CameraEvent::kCodeImage ||
-			   e->getCode() == rtabmap::CameraEvent::kCodeImageDepth)
+			if(e->getCode() == rtabmap::CameraEvent::kCodeData)
 			{
 				processingData_ = true;
 				QMetaObject::invokeMethod(this, "processImages",
-						Q_ARG(cv::Mat, e->data().image()),
-						Q_ARG(cv::Mat, e->data().depthOrRightImage()),
+						Q_ARG(cv::Mat, e->data().imageRaw()),
+						Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
 						Q_ARG(QString, QString(e->cameraName().c_str())));
 			}
 		}
@@ -287,6 +291,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 
 	std::vector<std::vector<cv::Point2f> > pointBuf(2);
 
+	bool depthDetected = false;
 	for(int id=0; id<(stereo_?2:1); ++id)
 	{
 		cv::Mat viewGray;
@@ -294,6 +299,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		{
 			if(images[id].type() == CV_16UC1)
 			{
+				depthDetected = true;
 				//assume IR image: convert to gray scaled
 				const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
 				viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
@@ -488,6 +494,8 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 			}
 		}
 	}
+	ui_->label_baseline->setVisible(!depthDetected);
+	ui_->label_baseline_name->setVisible(!depthDetected);
 
 	if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
 	{
@@ -516,7 +524,10 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 			images[1] = models_[1].rectifyImage(images[1]);
 		}
 	}
-	else if(ui_->radioButton_stereoRectified->isChecked() && stereoModel_.isValid())
+	else if(ui_->radioButton_stereoRectified->isChecked() &&
+			(stereoModel_.left().isValid() &&
+			stereoModel_.right().isValid()&&
+			(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
 	{
 		images[0] = stereoModel_.left().rectifyImage(images[0]);
 		images[1] = stereoModel_.right().rectifyImage(images[1]);
@@ -736,12 +747,27 @@ void CalibrationDialog::calibrate()
 		objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
 
 		// calibrate extrinsic
-		double rms = cv::stereoCalibrate(objectPoints, stereoImagePoints_[0], stereoImagePoints_[1],
-						models_[0].K(), models_[0].D(),
-						models_[1].K(), models_[1].D(),
-						imageSize, R, T, E, F,
-						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
-						cv::CALIB_FIX_INTRINSIC);
+#if CV_MAJOR_VERSION < 3
+		double rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				models_[0].K(), models_[0].D(),
+				models_[1].K(), models_[1].D(),
+				imageSize, R, T, E, F,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
+				cv::CALIB_FIX_INTRINSIC);
+#else
+		double rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				models_[0].K(), models_[0].D(),
+				models_[1].K(), models_[1].D(),
+				imageSize, R, T, E, F,
+				cv::CALIB_FIX_INTRINSIC,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+#endif
 		UINFO("stereo calibration... done with RMS error=%f", rms);
 
 		double err = 0;
@@ -818,7 +844,10 @@ void CalibrationDialog::calibrate()
 		//ui_->label_error_stereo->setNum(totalAvgErr);
 	}
 
-	if(stereo_ && stereoModel_.isValid())
+	if(stereo_ &&
+		stereoModel_.left().isValid() &&
+		stereoModel_.right().isValid()&&
+		(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))
 	{
 		ui_->radioButton_rectified->setEnabled(true);
 		ui_->radioButton_stereoRectified->setEnabled(true);
@@ -848,7 +877,10 @@ bool CalibrationDialog::save()
 
 		if(!filePath.isEmpty())
 		{
-			if(models_[0].save(filePath.toStdString()))
+			QString name = QFileInfo(filePath).baseName();
+			QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
+			models_[0].setName(name.toStdString());
+			if(models_[0].save(dir.toStdString()))
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
 				UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
@@ -863,18 +895,21 @@ bool CalibrationDialog::save()
 	}
 	else
 	{
-		UASSERT(stereoModel_.isValid());
+		UASSERT(stereoModel_.left().isValid() &&
+				stereoModel_.right().isValid()&&
+				(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
 		QString cameraName = stereoModel_.name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
 		QString name = QFileInfo(filePath).baseName();
 		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
 		if(!name.isEmpty())
 		{
+			stereoModel_.setName(name.toStdString());
 			std::string base = (dir+QDir::separator()+name).toStdString();
 			std::string leftPath = base+"_left.yaml";
 			std::string rightPath = base+"_right.yaml";
 			std::string posePath = base+"_pose.yaml";
-			if(stereoModel_.save(dir.toStdString(), name.toStdString()))
+			if(stereoModel_.save(dir.toStdString(), false))
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
 						arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));