GraphViewer: added Show/Hide global path

Added labeling mechanism of nodes (by default the first node of a map is tagged "map#")

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 8)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 12)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -32,6 +32,8 @@ SET(PROJECT_VERSION_PATCH ${RTABMAP_PATCH_VERSION})
 
 SET(PROJECT_SOVERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 
+SET(RTABMAP_QT_VERSION 4 CACHE STRING "Which QT version to use")
+
 ####### COMPILATION PARAMS #######
 # In case of Makefiles if the user does not setup CMAKE_BUILD_TYPE, assume it's Release:
 IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
@@ -94,6 +96,14 @@ SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
 
+# Avoid Visual Studio bin/Release and bin/Debug sub directories
+SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
+SET( CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
+SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}")
+SET( CMAKE_LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}")
+SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
+
 ####### INSTALL DIR #######
 set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
   "Installation directory for header files")
@@ -124,14 +134,22 @@ IF("${VTK_MAJOR_VERSION}" EQUAL 5)
 ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
 FIND_PACKAGE(ZLIB REQUIRED)
 FIND_PACKAGE(Freenect)
+FIND_PACKAGE(freenect2 QUIET)
 FIND_PACKAGE(OpenNI2)
+FIND_PACKAGE(DC1394)
+FIND_PACKAGE(G2O)
+FIND_PACKAGE(FlyCapture2)
 
 # If Qt is here, the GUI will be built
-FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) 
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+ELSE()
+    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+ENDIF()
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
-  IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+  IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
        
     # Required when packaging, and set CMAKE_INSTALL_PREFIX to "/".
     SET(CMAKE_INSTALL_PREFIX "/")
@@ -163,11 +181,11 @@ CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREF
 ADD_SUBDIRECTORY( utilite )
 ADD_SUBDIRECTORY( corelib )
 
-IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
+IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND))
    ADD_SUBDIRECTORY( guilib )
    ADD_SUBDIRECTORY( app )
 ELSE()
-   MESSAGE(STATUS "[WARNING] Qt4 not found, the GUI lib and the stand-alone application will not be compiled...")
+   MESSAGE(STATUS "[WARNING] Qt not found, the GUI lib and the stand-alone application will not be compiled...")
 ENDIF()
 
 ADD_SUBDIRECTORY( tools )
@@ -328,4 +346,36 @@ ELSE()
 MESSAGE(STATUS "  With OpenNI2  = NO (OpenNI2 not found)")
 ENDIF()
 
+IF(freenect2_FOUND)
+MESSAGE(STATUS "  With Freenect2 = YES")
+ELSE()
+MESSAGE(STATUS "  With Freenect2 = NO (libfreenect2 not found)")
+ENDIF()
+
+IF(DC1394_FOUND)
+MESSAGE(STATUS "  With dc1394  = YES")
+ELSE()
+MESSAGE(STATUS "  With dc1394  = NO (dc1394 not found)")
+ENDIF()
+
+IF(FlyCapture2_FOUND)
+MESSAGE(STATUS "  With FlyCapture2/Triclops  = YES")
+ELSE()
+MESSAGE(STATUS "  With FlyCapture2/Triclops  = NO (Point Grey SDK not found)")
+ENDIF()
+
+IF(G2O_FOUND)
+MESSAGE(STATUS "  With g2o  = YES")
+ELSE()
+MESSAGE(STATUS "  With g2o  = NO (g2o not found)")
+ENDIF()
+
+IF(QT4_FOUND)
+MESSAGE(STATUS "  With Qt  = YES (version 4)")
+ELSEIF(Qt5_FOUND)
+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)")
+ENDIF()
+
 MESSAGE(STATUS "--------------------------------------------")

app/src/CMakeLists.txt

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

app/src/main.cpp

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

bin/.gitignore

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

build/.gitignore

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

cmake_modules/FindDC1394.cmake

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

cmake_modules/FindFlyCapture2.cmake

@@ -0,0 +1,36 @@
+# - Find FlyCapture2 
+# This module finds an installed FlyCapture2+Triclops stereo camera package. (Point Grey SDK)
+#
+# It sets the following variables:
+#  FlyCapture2_FOUND       - Set to false, or undefined, if FlyCapture2 isn't found.
+#  FlyCapture2_INCLUDE_DIRS - The FlyCapture2 include directory.
+#  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
+
+#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)
+
+# 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)
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+   SET(FlyCapture2_FOUND TRUE)
+   SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+
+IF (FlyCapture2_FOUND)
+   # show which FlyCapture2 was found only if not quiet
+   IF (NOT FlyCapture2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_LIBRARIES}")
+   ENDIF (NOT FlyCapture2_FIND_QUIETLY)
+ELSE (FlyCapture2_FOUND)
+   # fatal error if FlyCapture2 is required but not found
+   IF (FlyCapture2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FlyCapture2 (FlyCapture2 Stereo Vision SDK)")
+   ENDIF (FlyCapture2_FIND_REQUIRED)
+ENDIF (FlyCapture2_FOUND)
+

cmake_modules/FindG2O.cmake

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

corelib/include/rtabmap/core/Camera.h

@@ -56,10 +56,16 @@ public:
 	//getters
 	void getImageSize(unsigned int & width, unsigned int & height);
 	float getImageRate() const {return _imageRate;}
+	bool isMirroringEnabled() const {return _mirroring;}
 
 	//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();
 
 protected:
 	/**
@@ -77,7 +83,10 @@ private:
 	float _imageRate;
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
+	bool _mirroring;
 	UTimer * _frameRateTimer;
+	cv::Mat _k; // camera_matrix
+	cv::Mat _d; // distorsion_coefficients
 };
 
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -44,9 +44,10 @@ public:
 	};
 
 public:
-	CameraEvent(const cv::Mat & image, int seq=0) :
+	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = "") :
 		UEvent(kCodeImage),
-		data_(image, seq)
+		data_(image, seq, stamp),
+		cameraName_(cameraName)
 	{
 	}
 
@@ -55,26 +56,23 @@ public:
 	{
 	}
 
-	CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
+	CameraEvent(const SensorData & data, const std::string & cameraName = "") :
 		UEvent(kCodeImageDepth),
-		data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
+		data_(data),
-	{
+		cameraName_(cameraName)
-	}
-
-	CameraEvent(const SensorData & data) :
-		UEvent(kCodeImageDepth),
-		data_(data)
 	{
 	}
 
 	// Image or descriptors
 	const SensorData & data() const {return data_;}
+	const std::string & cameraName() const {return cameraName_;}
 
 	virtual ~CameraEvent() {}
 	virtual std::string getClassName() const {return std::string("CameraEvent");}
 
 private:
 	SensorData data_;
+	std::string cameraName_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

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

corelib/include/rtabmap/core/CameraRGBD.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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>
@@ -54,7 +55,21 @@ class VideoStream;
 
 namespace pcl
 {
-	class Grabber;
+class Grabber;
+}
+
+namespace libfreenect2
+{
+class Freenect2;
+class Freenect2Device;
+class SyncMultiFrameListener;
+class Registration;
+class PacketPipeline;
+}
+
+namespace FlyCapture2
+{
+class Camera;
 }
 
 typedef struct _freenect_context freenect_context;
@@ -72,23 +87,22 @@ class RTABMAP_EXP CameraRGBD
 public:
 	virtual ~CameraRGBD();
 	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
-	virtual bool init() = 0;
+
+	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;}
-	float getFx() const {return _fx;}
+	bool isMirroringEnabled() const {return _mirroring;}
-	float getFy() const {return _fy;}
+	bool isColorOnly() const {return _colorOnly;}
-	float getCx() const {return _cx;}
-	float getCy() const {return _cy;}
 
 	//setters
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-	void setFx(float fx) {_fx = fx;}
+	void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
-	void setFy(float fy) {_fy = fy;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
-	void setCx(float cx) {_cx = cx;}
-	void setCy(float cy) {_cy = cy;}
 
 protected:
 	/**
@@ -97,22 +111,19 @@ protected:
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
 	CameraRGBD(float imageRate = 0,
-				const Transform & localTransform = Transform::getIdentity(),
+				const Transform & localTransform = Transform::getIdentity());
-				float fx = 0.0f,
-				float fy = 0.0f,
-				float cx = 0.0f,
-				float cy = 0.0f);
 
+	/**
+	 * 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;
-	float _fx;
-	float _fy;
-	float _cx;
-	float _cy;
 };
 
 /////////////////////////
@@ -128,11 +139,7 @@ public:
 	// default local transform z in, x right, y down));
 	CameraOpenni(const std::string & deviceId="",
 			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = Transform::getIdentity());
-			float fx = 0.0f,
-			float fy = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f);
 	virtual ~CameraOpenni();
 
     void image_cb (
@@ -140,7 +147,9 @@ public:
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
 			float constant);
 
-    bool init();
+    virtual bool init(const std::string & calibrationFolder = ".");
+    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);
@@ -169,14 +178,12 @@ public:
 public:
 	CameraOpenNICV(bool asus = false,
 					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity(),
+					const Transform & localTransform = Transform::getIdentity());
-					float fx = 0.0f,
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
 	virtual ~CameraOpenNICV();
 
-	virtual bool init();
+	virtual bool init(const std::string & calibrationFolder = ".");
+	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);
@@ -199,20 +206,20 @@ public:
 	static bool exposureGainAvailable();
 
 public:
-	CameraOpenNI2(float imageRate = 0,
+	CameraOpenNI2(const std::string & deviceId = "",
-					const Transform & localTransform = Transform::getIdentity(),
+					float imageRate = 0,
-					float fx = 0.0f,
+					const Transform & localTransform = Transform::getIdentity());
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
 	virtual ~CameraOpenNI2();
 
-	virtual bool init();
+	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
 
 	bool setAutoWhiteBalance(bool enabled);
 	bool setAutoExposure(bool enabled);
 	bool setExposure(int value);
 	bool setGain(int value);
+	bool setMirroring(bool enabled);
 
 protected:
 	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
@@ -223,6 +230,7 @@ private:
 	openni::VideoStream * _depth;
 	float _depthFx;
 	float _depthFy;
+	std::string _deviceId;
 };
 
 
@@ -241,14 +249,12 @@ public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,
 					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
+					const Transform & localTransform = Transform::getIdentity());
-					float fx = 0.0f,
-					float fy = 0.0f,
-					float cx = 0.0f,
-					float cy = 0.0f);
 	virtual ~CameraFreenect();
 
-	bool init();
+	virtual bool init(const std::string & calibrationFolder = ".");
+	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);
@@ -259,4 +265,99 @@ private:
 	FreenectDevice * freenectDevice_;
 };
 
+/////////////////////////
+// CameraFreenect2
+/////////////////////////
+
+class RTABMAP_EXP CameraFreenect2 :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+	enum Type{
+		kTypeRGBDepthSD,
+		kTypeRGBDepthHD,
+		kTypeIRDepth,
+		kTypeRGBIR
+	};
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect2(int deviceId= 0,
+					Type type = kTypeRGBDepthSD,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraFreenect2();
+
+	virtual bool init(const std::string & calibrationFolder = ".");
+	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);
+
+private:
+	int deviceId_;
+	Type type_;
+	StereoCameraModel stereoModel_;
+	libfreenect2::Freenect2 * freenect2_;
+	libfreenect2::Freenect2Device *dev_;
+	libfreenect2::PacketPipeline * pipeline_;
+	libfreenect2::SyncMultiFrameListener * listener_;
+	libfreenect2::Registration * reg_;
+};
+
+/////////////////////////
+// CameraStereoDC1394
+/////////////////////////
+class DC1394Device;
+
+class RTABMAP_EXP CameraStereoDC1394 :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoDC1394();
+
+	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:
+	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
+};
+
 } // namespace rtabmap

corelib/include/rtabmap/core/Compression.h

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

corelib/include/rtabmap/core/DBDriver.h

@@ -72,14 +72,13 @@ public:
 	void asyncSave(VisualWord * vw); //ownership transferred
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
 
 public:
 	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
-	bool getSignature(int signatureId, Signature ** s);
-	bool getVisualWord(int wordId, VisualWord ** vw);
 
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection();
@@ -96,15 +95,17 @@ public:
 
 	// 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) 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;
-	void getPose(int signatureId, Transform & pose, int & mapId) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) 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 getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodeIdByLabel(const std::string & label, int & id) const;
+	void getAllLabels(std::map<int, std::string> & labels) const;
 
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -121,8 +122,8 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
 
 
 	// Load objects
@@ -133,12 +134,14 @@ private:
 	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) 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 void getPoseQuery(int signatureId, Transform & pose, int & mapId) 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 getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 
 private:
 	//non-abstract methods
@@ -157,6 +160,7 @@ private:
 	USemaphore _addSem;
 	double _emptyTrashesTime;
 	std::string _url;
+	bool _timestampUpdate;
 };
 
 }

corelib/include/rtabmap/core/DBReader.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 
 #include <set>
+#include <list>
 
 namespace rtabmap {
 
@@ -49,7 +50,11 @@ public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
 			 bool odometryIgnored = false,
-			 float delayToStartSec = 0.0f);
+			 bool ignoreGoalDelay = false);
+	DBReader(const std::list<std::string> & databasePaths,
+			 float frameRate = 0.0f,
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -61,15 +66,16 @@ protected:
 	virtual void mainLoop();
 
 private:
-	std::string _path;
+	std::list<std::string> _paths;
-	float _frameRate;
+	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
-	float _delayToStartSec;
+	bool _ignoreGoalDelay;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;
 	std::set<int> _ids;
 	std::set<int>::iterator _currentId;
+	double _previousStamp;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Features2d.h

@@ -89,21 +89,26 @@ public:
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 
+	int getMaxFeatures() const {return maxFeatures_;}
+
 public:
 	virtual ~Feature2D() {}
 
-	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
 	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
-	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual void parseParameters(const ParametersMap & parameters);
 	virtual Feature2D::Type getType() const = 0;
 
 protected:
-	Feature2D(const ParametersMap & parameters = ParametersMap()) {}
+	Feature2D(const ParametersMap & parameters = ParametersMap());
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+
+private:
+	int maxFeatures_;
 };
 
 //SURF

corelib/include/rtabmap/core/Graph.h

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

corelib/include/rtabmap/core/Link.h

@@ -29,27 +29,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LINK_H_
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
 
 namespace rtabmap {
 
 class Link
 {
 public:
-	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
+	enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
 	Link() :
 		from_(0),
 		to_(0),
 		type_(kUndef),
-		variance_(1.0f)
+		rotVariance_(1.0f),
+		transVariance_(1.0f)
 	{
 	}
-	Link(int from, int to, Type type, const Transform & transform, float variance) :
+	Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
 		from_(from),
 		to_(to),
 		transform_(transform),
 		type_(type),
-		variance_(variance)
+		rotVariance_(rotVariance),
+		transVariance_(transVariance)
 	{
+		UASSERT_MSG(uIsFinite(rotVariance) && rotVariance>0 && uIsFinite(transVariance) && transVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
 	}
 
 	bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
@@ -58,20 +63,26 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
-	float variance() const {return variance_;}
+	float rotVariance() const {return rotVariance_;}
+	float transVariance() const {return transVariance_;}
 
 	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 variance) {variance_ = variance;}
+	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;
+	}
 
 private:
 	int from_;
 	int to_;
 	Transform transform_;
 	Type type_;
-	float variance_;
+	float rotVariance_;
+	float transVariance_;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -73,6 +73,7 @@ public:
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
 	int incrementMapId();
+	void updateAge(int signatureId);
 
 	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
@@ -80,38 +81,56 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance);
+	bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
-	void updateNeighborLink(int fromId, int toId, const Transform & transform, float variance);
+	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
-	std::map<int, int> getNeighborsId(int signatureId,
+	void removeAllVirtualLinks();
-			int margin,
+	std::map<int, int> getNeighborsId(
+			int signatureId,
+			int maxGraphDepth,
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
+	std::map<int, float> getNeighborsIdRadius(
+			int signatureId,
+			float radius,
+			const std::map<int, Transform> & optimizedPoses,
+			int maxGraphDepth) const;
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
-	void rejectLoopClosure(int oldId, int newId);
+	void removeLink(int idA, int idB);
 
 	//getters
-	const std::set<int> & getWorkingMem() const {return _workingMem;}
+	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
-	void getPose(int locationId,
-			Transform & pose,
-			bool lookInDatabase = false) const;
 	std::map<int, Link> getNeighborLinks(int signatureId,
 			bool lookInDatabase = false) const;
 	std::map<int, Link> getLoopClosureLinks(int signatureId,
 			bool lookInDatabase = false) const;
 	bool isRawDataKept() const {return _rawDataKept;}
+	bool isBinDataKept() const {return _binDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
 	int getLastSignatureId() const;
 	const Signature * getLastWorkingSignature() const;
+	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
+	bool labelSignature(int id, const std::string & label);
+	std::map<int, std::string> getAllLabels() const;
+	bool setUserData(int id, const std::vector<unsigned char> & data);
 	int getDatabaseMemoryUsed() const; // in bytes
 	double getDbSavingTime() const;
-	int getMapId(int signatureId) const;
+	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
+	bool getNodeInfo(int signatureId,
+			Transform & odomPose,
+			int & mapId,
+			int & weight,
+			std::string & label,
+			double & stamp,
+			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;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -120,8 +139,7 @@ public:
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
 	bool isIDsGenerated() const {return _generateIds;}
-	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
+	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
-	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
 	const Feature2D * getFeature2D() const {return _feature2D;}
 
 	void setRoi(const std::string & roi);
@@ -142,7 +160,7 @@ public:
 
 	// RGB-D stuff
 	void getMetricConstraints(
-			const std::vector<int> & ids,
+			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);
@@ -153,8 +171,8 @@ public:
 	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;
-	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
+	Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
-	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
 	Transform computeScanMatchingTransform(
 			int newId,
 			int oldId,
@@ -165,9 +183,9 @@ public:
 
 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature, float odomVariance);
+	void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
 	void clear();
-	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
+	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
 
 	void addSignatureToWm(Signature * signature);
 	Signature * _getSignature(int id) const;
@@ -183,7 +201,6 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
-			bool keepRawData=false,
 			Statistics * stats = 0);
 
 	//keypoint stuff
@@ -199,26 +216,34 @@ private:
 	// parameters
 	float _similarityThreshold;
 	bool _rawDataKept;
-	bool _keepRehearsedNodesInDb;
+	bool _binDataKept;
+	bool _notLinkedNodesKeptInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
 	float _recentWmRatio;
+	bool _transferSortingByWeightId;
 	bool _idUpdatedToNewOneRehearsal;
 	bool _generateIds;
 	bool _badSignaturesIgnored;
+	int _imageDecimation;
+	float _laserScanVoxelSize;
+	bool _localSpaceLinksKeptInWM;
+	float _rehearsalMaxDistance;
+	float _rehearsalMaxAngle;
+	bool _rehearsalWeightIgnoredWhileMoving;
 
 	int _idCount;
 	int _idMapCount;
 	Signature * _lastSignature;
-	int _lastGlobalLoopClosureParentId;
+	int _lastGlobalLoopClosureId;
-	int _lastGlobalLoopClosureChildId;
+	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
-	bool _memoryChanged; // False by default, become true when Memory::update() is called.
+	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	bool _postInitClosingEvents;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
-	std::set<int> _workingMem; // id,age
+	std::map<int, double> _workingMem; // id,age
 
 	//Keypoint stuff
 	VWDictionary * _vwd;
@@ -228,7 +253,6 @@ private:
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
 	float _wordsMaxDepth; // 0=inf
-	int _wordsPerImageTarget; // <0=none, 0=inf
 	std::vector<float> _roiRatios; // size 4
 
 	// RGBD-SLAM stuff
@@ -237,6 +261,10 @@ private:
 	int _bowIterations;
 	float _bowMaxDepth;
 	bool _bowForce2D;
+	bool _bowEpipolarGeometry;
+	float _bowEpipolarGeometryVar;
+	float _icpMaxTranslation;
+	float _icpMaxRotation;
 	int _icpDecimation;
 	float _icpMaxDepth;
 	float _icpVoxelSize;

corelib/include/rtabmap/core/Odometry.h

@@ -30,20 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UEventsHandler.h>
-#include <rtabmap/utilite/UEvent.h>
-#include <rtabmap/utilite/UMutex.h>
-#include <rtabmap/utilite/USemaphore.h>
-
-#include <rtabmap/core/Parameters.h>
-
 #include <rtabmap/core/SensorData.h>
-#include <rtabmap/core/OdometryInfo.h>
+#include <rtabmap/core/Parameters.h>
-
-#include <opencv2/opencv.hpp>
-
-#include <pcl/common/eigen.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 
@@ -52,6 +41,7 @@ class UTimer;
 namespace rtabmap {
 
 class Feature2D;
+class OdometryInfo;
 
 class RTABMAP_EXP Odometry
 {
@@ -62,7 +52,6 @@ public:
 
 	//getters
 	const Transform & getPose() const {return _pose;}
-	int getMaxFeatures() const  {return _maxFeatures;}
 	const std::string & getRoiRatios() const {return _roiRatios;}
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
@@ -70,12 +59,14 @@ public:
 	int getRefineIterations() const {return _refineIterations;}
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
+	bool isPnPEstimationUsed() const {return _pnpEstimation;}
+	double getPnPReprojError() const {return _pnpReprojError;}
+	int  getPnPFlags() const {return _pnpFlags;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
 
 private:
-	int _maxFeatures;
 	std::string _roiRatios;
 	int _minInliers;
 	float _inlierDistance;
@@ -85,6 +76,9 @@ private:
 	int _resetCountdown;
 	bool _force2D;
 	bool _fillInfoData;
+	bool _pnpEstimation;
+	double _pnpReprojError;
+	int _pnpFlags;
 	Transform _pose;
 	int _resetCurrentCount;
 
@@ -157,6 +151,38 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
 };
 
+class RTABMAP_EXP OdometryMono : public Odometry
+{
+public:
+	OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryMono();
+	virtual void reset(const Transform & initialPose);
+
+private:
+	virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
+private:
+	//Parameters:
+	int flowWinSize_;
+	int flowIterations_;
+	double flowEps_;
+	int flowMaxLevel_;
+
+	Memory * memory_;
+	int localHistoryMaxSize_;
+	float initMinFlow_;
+	float initMinTranslation_;
+	float minTranslation_;
+	float fundMatrixReprojError_;
+	float fundMatrixConfidence_;
+
+	cv::Mat refDepth_;
+	std::map<int, cv::Point2f> cornersMap_;
+	std::multimap<int, cv::Point3f> localMap_;
+	std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
+	std::map<int, Transform> keyFramePoses_;
+	float maxVariance_;
+};
+
 class RTABMAP_EXP OdometryICP : public Odometry
 {
 public:
@@ -186,32 +212,5 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
 };
 
-class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
-public:
-	// take ownership of Odometry
-	OdometryThread(Odometry * odometry);
-	virtual ~OdometryThread();
-
-protected:
-	virtual void handleEvent(UEvent * event);
-
-private:
-	void mainLoopKill();
-
-	//============================================================
-	// MAIN LOOP
-	//============================================================
-	void mainLoop();
-	void addData(const SensorData & data);
-	void getData(SensorData & data);
-
-private:
-	USemaphore _dataAdded;
-	UMutex _dataMutex;
-	SensorData _dataBuffer;
-	Odometry * _odometry;
-	bool _resetOdometry;
-};
-
 } /* namespace rtabmap */
 #endif /* ODOMETRY_H_ */

corelib/include/rtabmap/core/OdometryInfo.h

@@ -57,10 +57,11 @@ public:
 	std::multimap<int, cv::KeyPoint> words;
 	std::vector<int> wordMatches;
 	std::vector<int> wordInliers;
+	std::multimap<int, cv::Point3f> localMap;
 
 	// Optical Flow odometry
-	std::vector<cv::KeyPoint> refCorners;
+	std::vector<cv::Point2f> refCorners;
-	std::vector<cv::KeyPoint> newCorners;
+	std::vector<cv::Point2f> newCorners;
 	std::vector<int> cornerInliers;
 };
 

corelib/include/rtabmap/core/OdometryThread.h

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

corelib/include/rtabmap/core/Parameters.h

@@ -175,7 +175,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");
 	RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,   	 bool, true, "Add column header description to log files.");
-	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.")
+	RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
@@ -183,15 +183,22 @@ class RTABMAP_EXP Parameters
 
 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
-	RTABMAP_PARAM(Mem, ImageKept, 		        bool, true, 	"Keep images in db.");
+	RTABMAP_PARAM(Mem, ImageKept, 		        bool, false, 	"Keep raw images in RAM.");
-	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true, 	"Keep rehearsed ndoes in db.");
+	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
+	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
-	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, othwersize it is Localization mode.");
+	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, otherwise it is Localization mode.");
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
+	RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false,  "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
-	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location Ids, False=use input image ids.");
+	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
+	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
-	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1) when creating a signature.");
+	RTABMAP_PARAM(Mem, LaserScanVoxelSize,      float, 0.0,      "If > 0.0, voxelize laser scans when creating a signature.");
+	RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true,      "If local space links are kept in WM.");
+
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM_COND(Kp, NNStrategy,       int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -240,14 +247,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
 
-	RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
 	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
 	RTABMAP_PARAM(GFTT, K, double, 0.04, "");
 
-	RTABMAP_PARAM(ORB, NFeatures,            int, 400,     "The maximum number of features to retain.");
 	RTABMAP_PARAM(ORB, ScaleFactor,          float,  1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
 	RTABMAP_PARAM(ORB, NLevels,              int, 1,       "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
 	RTABMAP_PARAM(ORB, EdgeThreshold,        int, 31,      "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
@@ -282,21 +287,31 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
-	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations");
-	RTABMAP_PARAM(RGBD, ToroIgnoreVariance,   bool, false,    "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
 	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, 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.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
+
+	// Graph optimization
+	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,        "Graph optimization strategy: 0=TORO and 1=g2o.");
+	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,      "Optimization iterations.");
+	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
+	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
 
 	// 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, MaxFeatures,            int, 0, 		"0 no limits.");
+	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.");
@@ -305,12 +320,22 @@ class RTABMAP_EXP Parameters
 	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, FillInfoData, 		    bool, false,     "Fill info with data (inliers/outliers features).");
+	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");
 
 	// 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.9,  "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
+
+	// Odometry Mono
+	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, InitMinTranslation,     float, 0.1,  "Minimum translation required for the initialization step.");
+	RTABMAP_PARAM(OdomMono, MinTranslation,         float, 0.02,  "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
+	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01,  "Maximum variance to add new points to local map.");
+
 
 	// Odometry common stuff between BOW and Optical Flow approaches
 	RTABMAP_PARAM(OdomFlow, WinSize,               int, 16,       "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
@@ -324,16 +349,19 @@ class RTABMAP_EXP Parameters
 
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
-	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxTranslation,  float, 0.2,      "Maximum ICP translation correction accepted (m).");
+	RTABMAP_PARAM(LccIcp, MaxRotation,     float, 0.78,     "Maximum ICP rotation correction accepted (rad).");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(LccBow, 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, 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, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
 	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.9, 	"NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.8, 	"NNDR: nearest neighbor distance ratio.");
 	RTABMAP_PARAM(LccReextract, FeatureType, int, 4, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
 	RTABMAP_PARAM(LccReextract, MaxWords, 	int, 600, 		"0 no limits.");
 
@@ -342,15 +370,15 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.01, 	"Voxel size to be used for ICP computation.");
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
+	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"ICP 3D: Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"ICP 3D: Use point to plane ICP.");
+	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
-	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"ICP 3D: Number of neighbors to compute normals for point to plane.");
+	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
 
-	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, 	"Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
+	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, 		"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.025, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity
 	RTABMAP_PARAM(Stereo, WinSize,               int, 16,        "See cv::calcOpticalFlowPyrLK().");

corelib/include/rtabmap/core/Rtabmap.h

@@ -47,6 +47,9 @@ class EpipolarGeometry;
 class Memory;
 class BayesFilter;
 class Signature;
+namespace graph {
+class Optimizer;
+}
 
 class RTABMAP_EXP Rtabmap
 {
@@ -71,10 +74,11 @@ public:
 	void close();
 
 	const std::string & getWorkingDir() const {return _wDir;}
-	int getLoopClosureId() const;
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
-	int getRetrievedId() const;
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
 	int getLastLocationId() const;
-	float getLcHypValue() const {return _lcHypothesisValue;}
 	std::list<int> getWM() const; // working memory
 	std::set<int> getSTM() const; // short-term memory
 	int getWMSize() const; // working memory size
@@ -87,14 +91,19 @@ public:
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
+	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
+	float getLocalRadius() const {return _localRadius;}
 
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
-	void triggerNewMap();
+	int triggerNewMap();
+	bool labelLocation(int id, const std::string & label);
+	bool setUserData(int id, const std::vector<unsigned char> & data);
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
 	void generateTOROGraph(const std::string & path, bool optimized, bool global);
 	void resetMemory();
@@ -102,21 +111,35 @@ public:
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
-	void deleteLocation(int locationId); // Only nodes in STM can be deleted
 	void rejectLoopClosure(int oldId, int newId);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			std::map<int, int> & mapIds,
+			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);
+	void clearPath();
+	bool computePath(int targetNode, bool global);
+	bool computePath(const Transform & targetPose, bool global);
+	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
+	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<int> getPathNextNodes() const;
+	int getPathCurrentGoalId() const;
+	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
-	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
+	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
+	std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;
@@ -126,6 +149,13 @@ private:
 			bool lookInDatabase,
 			std::map<int, Transform> & optimizedPoses,
 			std::multimap<int, Link> * constraints = 0) const;
+	std::map<int, Transform> optimizeGraph(
+			int fromId,
+			const std::set<int> & ids,
+			bool lookInDatabase,
+			std::multimap<int, Link> * constraints = 0) const;
+	void updateGoalIndex();
+	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
@@ -141,6 +171,7 @@ private:
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
+	unsigned int _maxLocalRetrieved;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
 	bool _statisticLoggedHeaders;
@@ -149,15 +180,14 @@ private:
 	float _rgbdAngularUpdate;
 	float _newMapOdomChangeDistance;
 	int _globalLoopClosureIcpType;
-	float _globalLoopClosureIcpMaxDistance;
 	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
-	float _localDetectRadius;
+	float _localRadius;
-	float _localDetectMaxNeighbors;
+	float _localImmunizationRatio;
-	int _localDetectMaxDiffID;
+	int _localDetectMaxGraphDepth;
-	int _toroIterations;
+	float _localPathFilteringRadius;
-	bool _toroIgnoreVariance;
+	bool _localPathOdomPosesUsed;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	bool _reextractLoopClosureFeatures;
@@ -166,17 +196,20 @@ private:
 	int _reextractFeatureType;
 	int _reextractMaxWords;
 	bool _startNewMapOnLoopClosure;
+	float _goalReachedRadius; // meters
+	bool _planVirtualLinks;
+	bool _goalsSavedInUserData;
 
-	int _lcHypothesisId;
+	std::pair<int, float> _loopClosureHypothesis;
-	float _lcHypothesisValue;
+	std::pair<int, float> _highestHypothesis;
-	int _retrievedId;
 	double _lastProcessTime;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
-	ParametersMap _lastParameters;
+	graph::Optimizer * _graphOptimizer;
+	ParametersMap _modifiedParameters;
 
 	Memory * _memory;
 
@@ -193,6 +226,14 @@ private:
 	std::multimap<int, Link> _constraints;
 	Transform _mapCorrection;
 	Transform _mapTransform; // for localization mode
+	Transform _lastLocalizationPose; // for localization mode
+
+	// Planning stuff
+	std::vector<std::pair<int,Transform> > _path;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+	Transform _pathTransformToGoal;
+
 };
 
 #endif /* RTABMAP_H_ */

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -73,7 +73,8 @@ public:
 			kCmdPublishTOROGraphGlobal, // params: optimized
 			kCmdPublishTOROGraphLocal, // params: optimized
 			kCmdTriggerNewMap,
-			kCmdPause};
+			kCmdPause,
+			kCmdGoal}; // params: label or location ID
 public:
 	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),
@@ -148,12 +149,18 @@ public:
 			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, int> & mapIds,
+			const std::map<int, double> & stamps,
+			const std::map<int, std::string> & labels,
+			const std::map<int, std::vector<unsigned char> > & userDatas) :
 		UEvent(0),
 		_signatures(signatures),
 		_poses(poses),
 		_constraints(constraints),
-		_mapIds(mapIds)
+		_mapIds(mapIds),
+		_stamps(stamps),
+		_labels(labels),
+		_userDatas(userDatas)
 	{}
 
 	virtual ~RtabmapEvent3DMap() {}
@@ -162,6 +169,9 @@ public:
 	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");}
 
@@ -170,6 +180,27 @@ private:
 	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
+{
+public:
+	RtabmapGlobalPathEvent():
+		UEvent(0) {}
+	RtabmapGlobalPathEvent(int goalId, const std::vector<std::pair<int, Transform> > & poses) :
+		UEvent(goalId),
+		_poses(poses) {}
+
+	virtual ~RtabmapGlobalPathEvent() {}
+	int getGoal() const {return this->getCode();}
+	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
+	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
+
+private:
+	std::vector<std::pair<int, Transform> > _poses;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/RtabmapThread.h

@@ -69,7 +69,9 @@ public:
 		kStatePublishingMapGlobal,
 		kStatePublishingTOROGraphLocal,
 		kStatePublishingTOROGraphGlobal,
-		kStateTriggeringMap
+		kStateTriggeringMap,
+		kStateAddingUserData,
+		kStateSettingGoal
 	};
 
 public:
@@ -110,7 +112,11 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	float _variance;
+	float _rotVariance;
+	float _transVariance;
+
+	std::vector<unsigned char> _userData;
+	UMutex _userDataMutex;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/SensorData.h

@@ -43,7 +43,7 @@ class RTABMAP_EXP SensorData
 {
 public:
 	SensorData(); // empty constructor
-	SensorData(const cv::Mat & image, int id = 0);
+	SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	// Metric constructor
 	SensorData(const cv::Mat & image,
@@ -54,11 +54,15 @@ public:
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id = 0);
+		  float poseTransVariance,
+		  int id,
+		  double stamp,
+		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	// Metric constructor + 2d laser scan
 	SensorData(const cv::Mat & laserScan,
+			int laserScanMaxPts,
 		  const cv::Mat & image,
 		  const cv::Mat & depthOrRightImage,
 		  float fx,
@@ -67,8 +71,11 @@ public:
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id = 0);
+		  float poseTransVariance,
+		  int id,
+		  double stamp,
+		  const std::vector<unsigned char> & userData = std::vector<unsigned char>());
 
 	virtual ~SensorData() {}
 
@@ -80,13 +87,16 @@ public:
 	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 variance) {_pose = pose; _poseVariance=variance;}
+	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;}
@@ -95,7 +105,8 @@ public:
 	float fyOrBaseline() const {return _fyOrBaseline;}
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}
-	float poseVariance() const {return _poseVariance;}
+	float poseRotVariance() const {return _poseRotVariance;}
+	float poseTransVariance() const {return _poseTransVariance;}
 
 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
@@ -105,9 +116,13 @@ 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;
@@ -118,11 +133,16 @@ private:
 	float _cy;
 	Transform _pose;
 	Transform _localTransform;
-	float _poseVariance;
+	float _poseRotVariance;
+	float _poseTransVariance;
+	int _laserScanMaxPts;
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;
+
+	// user data
+	std::vector<unsigned char> _userData;
 };
 
 }

corelib/include/rtabmap/core/Signature.h

@@ -54,9 +54,13 @@ 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,
 			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(),
@@ -64,7 +68,8 @@ public:
 			float fy = 0.0f,
 			float cx = 0.0f,
 			float cy = 0.0f,
-			const Transform & localTransform =Transform::getIdentity());
+			const Transform & localTransform =Transform::getIdentity(),
+			int laserScanMaxPts = 0);
 	virtual ~Signature();
 
 	/**
@@ -76,9 +81,17 @@ public:
 	int id() const {return _id;}
 	int mapId() const {return _mapId;}
 
-	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
+	void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
 	int getWeight() const {return _weight;}
 
+	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);
 	void addLinks(const std::map<int, Link> & links);
 	void addLink(const Link & link);
@@ -89,6 +102,7 @@ public:
 
 	void removeLinks();
 	void removeLink(int idTo);
+	void removeVirtualLinks();
 
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -115,22 +129,28 @@ public:
 	//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) {_laserScanCompressed = bytes;}
+	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;}
-	float getDepthFx() const {return _fx;}
+	RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead.");
-	float getDepthFy() const {return _fy;}
+	RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead.");
-	float getDepthCx() const {return _cx;}
+	RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead.");
-	float getDepthCy() const {return _cy;}
+	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) {_laserScanRaw = depth2D;}
+	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();
@@ -140,8 +160,11 @@ public:
 private:
 	int _id;
 	int _mapId;
+	double _stamp;
 	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
@@ -163,6 +186,7 @@ private:
 	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

corelib/include/rtabmap/core/Statistics.h

@@ -53,6 +53,7 @@ namespace rtabmap {
 class RTABMAP_EXP Statistics
 {
 	RTABMAP_STATS(Loop, RejectedHypothesis,);
+	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
@@ -60,23 +61,30 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
 	RTABMAP_STATS(Loop, VisualInliers,);
-	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
+	RTABMAP_STATS(Loop, Last_id,);
-	RTABMAP_STATS(Loop, Last_loop_closure_child,);
 
-	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
-	RTABMAP_STATS(LocalLoop, Space_closure_id,);
+	RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
-	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
+	RTABMAP_STATS(LocalLoop, Space_paths,);
-	RTABMAP_STATS(LocalLoop, Space_neighbors,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_visually,);
-	RTABMAP_STATS(LocalLoop, Space_diff_id,);
+	RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
+
+	RTABMAP_STATS(OdomCorrection, Accepted,);
+	RTABMAP_STATS(OdomCorrection, Inliers,);
+	RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
+	RTABMAP_STATS(OdomCorrection, Variance,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
 	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Immunized_globally,);
+	RTABMAP_STATS(Memory, Immunized_locally,);
+	RTABMAP_STATS(Memory, Immunized_locally_max,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);
 	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Local_graph_size,);
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Scan_matching, ms);
@@ -101,9 +109,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
 	RTABMAP_STATS(TimingMem, Rehearsal, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
-	RTABMAP_STATS(TimingMem, Stereo_subpixel, ms);
+	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
-	RTABMAP_STATS(TimingMem, Keypoints_filtering, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
@@ -130,6 +137,9 @@ public:
 	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 setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
@@ -140,6 +150,8 @@ public:
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
+	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
+	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -148,6 +160,9 @@ public:
 	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, Transform> & poses() const {return _poses;}
@@ -158,6 +173,8 @@ public:
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
+	const std::vector<int> & localPath() const {return _localPath;}
+	int currentGoalId() const {return _currentGoalId;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -170,6 +187,9 @@ private:
 
 	// 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;
@@ -184,6 +204,9 @@ private:
 	std::map<int, float> _likelihood;
 	std::map<int, float> _rawLikelihood;
 
+	std::vector<int> _localPath;
+	int _currentGoalId;
+
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/Transform.h

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <vector>
 #include <string>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 namespace rtabmap {
 
@@ -81,14 +83,19 @@ public:
 	const float & y() const {return data_[7];}
 	const float & z() const {return data_[11];}
 
+	float theta() const;
+
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;
 
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
 	void getTranslation(float & x, float & y, float & z) const;
 	float getNorm() const;
 	float getNormSquared() const;
+	float getDistance(const Transform & t) const;
+	float getDistanceSquared(const Transform & t) const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;
@@ -96,10 +103,22 @@ public:
 	bool operator==(const Transform & t) const;
 	bool operator!=(const Transform & t) const;
 
-	static Transform getIdentity()
+	Eigen::Matrix4f toEigen4f() const;
-	{
+	Eigen::Matrix4d toEigen4d() const;
-		return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
+	Eigen::Affine3f toEigen3f() const;
-	}
+	Eigen::Affine3d toEigen3d() const;
+
+	Eigen::Quaternionf getQuaternionf() const;
+	Eigen::Quaterniond getQuaterniond() const;
+
+public:
+	static Transform getIdentity();
+	static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
+	static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
+	static Transform fromEigen3f(const Eigen::Affine3f & matrix);
+	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
+	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
+	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
 
 private:
 	std::vector<float> data_;

corelib/include/rtabmap/core/UserDataEvent.h

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

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

@@ -127,7 +127,7 @@ typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
 	typedef typename pcl::PointCloud<PointT> PointCloud;
 	typedef typename PointCloud::Ptr PointCloudPtr;
 	PointCloudPtr output(new PointCloud);
-	pcl::transformPointCloud<PointT>(*cloud, *output, transformToEigen4f(transform));
+	pcl::transformPointCloud<PointT>(*cloud, *output, transform.toEigen4f());
 	return output;
 }
 
@@ -136,7 +136,7 @@ PointT transformPoint(
 		const PointT & pt,
 		const Transform & transform)
 {
-	return pcl::transformPoint(pt, transformToEigen3f(transform));
+	return pcl::transformPoint(pt, transform.toEigen3f());
 }
 
 template<typename PointT>
@@ -152,63 +152,66 @@ void segmentObstaclesFromGround(
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
 
-	// Find the ground
+	if(cloud->size())
-	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
-			cloud,
-			groundNormalAngle,
-			Eigen::Vector4f(0,0,1,0),
-			normalRadiusSearch*2.0f,
-			Eigen::Vector4f(0,0,100,0));
-
-	if(segmentFlatObstacles)
 	{
-		int biggestFlatSurfaceIndex;
+		// Find the ground
-		std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
+		pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
 				cloud,
-				flatSurfaces,
+				groundNormalAngle,
+				Eigen::Vector4f(0,0,1,0),
 				normalRadiusSearch*2.0f,
-				minClusterSize,
+				Eigen::Vector4f(0,0,100,0));
-				std::numeric_limits<int>::max(),
-				&biggestFlatSurfaceIndex);
 
-
+		if(segmentFlatObstacles)
-		// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
-		ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-		Eigen::Vector4f min,max;
-		pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
-
-		for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 		{
-			if((int)i!=biggestFlatSurfaceIndex)
+			int biggestFlatSurfaceIndex;
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
+					cloud,
+					flatSurfaces,
+					normalRadiusSearch*2.0f,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+
+
+			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
+			ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+			Eigen::Vector4f min,max;
+			pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+			for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 			{
-				Eigen::Vector4f centroid;
+				if((int)i!=biggestFlatSurfaceIndex)
-				pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-				if(centroid[2] >= min[2] && centroid[2] <= max[2])
 				{
-					ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+					Eigen::Vector4f centroid;
+					pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
+					if(centroid[2] >= min[2] && centroid[2] <= max[2])
+					{
+						ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+					}
 				}
 			}
 		}
-	}
+		else
-	else
+		{
-	{
+			ground = flatSurfaces;
-		ground = flatSurfaces;
+		}
-	}
 
-	if(ground->size() != cloud->size())
+		if(ground->size() != cloud->size())
-	{
+		{
-		// Remove ground
+			// Remove ground
-		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
+			pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
 
-		//Cluster remaining stuff (obstacles)
+			//Cluster remaining stuff (obstacles)
-		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
+			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
-				cloud,
+					cloud,
-				otherStuffIndices,
+					otherStuffIndices,
-				normalRadiusSearch*2.0f,
+					normalRadiusSearch*2.0f,
-				minClusterSize);
+					minClusterSize);
 
-		// merge indices
+			// merge indices
-		obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+		}
 	}
 }
 
@@ -302,51 +305,56 @@ pcl::IndicesPtr normalFiltering(
 		float radiusSearch,
 		const Eigen::Vector4f & viewpoint)
 {
-	typedef typename pcl::search::KdTree<PointT> KdTree;
+	pcl::IndicesPtr output(new std::vector<int>());
-	typedef typename KdTree::Ptr KdTreePtr;
 
-	pcl::NormalEstimation<PointT, pcl::Normal> ne;
+	if(cloud->size())
-	ne.setInputCloud (cloud);
-	if(indices->size())
 	{
-		ne.setIndices(indices);
+		typedef typename pcl::search::KdTree<PointT> KdTree;
-	}
+		typedef typename KdTree::Ptr KdTreePtr;
 
-	KdTreePtr tree (new KdTree(false));
+		pcl::NormalEstimation<PointT, pcl::Normal> ne;
-
+		ne.setInputCloud (cloud);
-	if(indices->size())
+		if(indices->size())
-	{
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud(cloud);
-	}
-	ne.setSearchMethod (tree);
-
-	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-
-	ne.setRadiusSearch (radiusSearch);
-	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
-	{
-		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
-	}
-
-	ne.compute (*cloud_normals);
-
-	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
-	int oi = 0; // output iterator
-	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
-	for(unsigned int i=0; i<cloud_normals->size(); ++i)
-	{
-		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
-		float angle = pcl::getAngle3D(normal, v);
-		if(angle < angleMax)
 		{
-			output->at(oi++) = indices->size()!=0?indices->at(i):i;
+			ne.setIndices(indices);
 		}
+
+		KdTreePtr tree (new KdTree(false));
+
+		if(indices->size())
+		{
+			tree->setInputCloud(cloud, indices);
+		}
+		else
+		{
+			tree->setInputCloud(cloud);
+		}
+		ne.setSearchMethod (tree);
+
+		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
+
+		ne.setRadiusSearch (radiusSearch);
+		if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
+		{
+			ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
+		}
+
+		ne.compute (*cloud_normals);
+
+		output->resize(cloud_normals->size());
+		int oi = 0; // output iterator
+		Eigen::Vector3f n(normal[0], normal[1], normal[2]);
+		for(unsigned int i=0; i<cloud_normals->size(); ++i)
+		{
+			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
+			float angle = pcl::getAngle3D(normal, v);
+			if(angle < angleMax)
+			{
+				output->at(oi++) = indices->size()!=0?indices->at(i):i;
+			}
+		}
+		output->resize(oi);
 	}
-	output->resize(oi);
 
 	return output;
 }
@@ -405,7 +413,7 @@ std::vector<pcl::IndicesPtr> extractClusters(
 
 		if(maxSize < cluster_indices[i].indices.size())
 		{
-			maxSize = cluster_indices[i].indices.size();
+			maxSize = (unsigned int)cluster_indices[i].indices.size();
 			maxIndex = i;
 		}
 	}
@@ -431,6 +439,72 @@ pcl::IndicesPtr extractNegativeIndices(
 	return output;
 }
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	if(cloud->size() == 0)
+	{
+		return;
+	}
+	pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+	segmentObstaclesFromGround<PointT>(cloud,
+			groundIndices,
+			obstaclesIndices,
+			cellSize,
+			groundNormalAngle,
+			minClusterSize);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
+		//voxelize to grid cell size
+		groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
+	}
+
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloud->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
+		}
+	}
+}
+
 } // util3d
 } // rtabmap
 #endif //UTIL3D_HPP_

corelib/include/rtabmap/core/util3d.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h"
 #include <opencv2/core/core.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include <string>
@@ -49,41 +50,6 @@ namespace rtabmap
 namespace util3d
 {
 
-/**
- * Compress image or data
- *
- * Example compression:
- *   cv::Mat image;// an image
- *   CompressionThread ct(image);
- *   ct.start();
- *   ct.join();
- *   std::vector<unsigned char> bytes = ct.getCompressedData();
- *
- * Example uncompression
- *   std::vector<unsigned char> bytes;// a compressed image
- *   CompressionThread ct(bytes);
- *   ct.start();
- *   ct.join();
- *   cv::Mat image = ct.getUncompressedData();
- */
-class RTABMAP_EXP CompressionThread : public UThread
-{
-public:
-	// format : ".png" ".jpg" "" (empty is general)
-	CompressionThread(const cv::Mat & mat, const std::string & format = "");
-	CompressionThread(const cv::Mat & bytes, bool isImage);
-	const cv::Mat & getCompressedData() const {return compressedData_;}
-	cv::Mat & getUncompressedData() {return uncompressedData_;}
-protected:
-	virtual void mainLoop();
-private:
-	cv::Mat compressedData_;
-	cv::Mat uncompressedData_;
-	std::string format_;
-	bool image_;
-	bool compressMode_;
-};
-
 cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
 cv::Mat RTABMAP_EXP depthFromCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
@@ -134,10 +100,33 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
 		int flowIterations = 20,
 		double flowEps = 0.02);
 
+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,
+		Transform & cameraTransform,
+		int pnpIterations = 100,
+		float pnpReprojError = 8.0f,
+		int pnpFlags = cv::ITERATIVE,
+		float ransacParam1 = 3.0f,
+		float ransacParam2 = 0.99f,
+		const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
+		double * variance = 0);
+
 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
+float RTABMAP_EXP getDepth(
+		const cv::Mat & depthImage,
+		float x, float y,
+		bool smoothing,
+		float maxZError = 0.02f);
+
 pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,
@@ -163,21 +152,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
 		const cv::Mat & imageDisparity,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
 		float cx, float cy,
 		float fx, float baseline,
-		int decimation);
+		int decimation = 1);
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
@@ -232,22 +221,17 @@ cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,
 		int type = CV_32FC1);
 
+cv::Mat RTABMAP_EXP registerDepth(
+		const cv::Mat & depth,
+		const cv::Mat & depthK,
+		const cv::Mat & colorK,
+		const rtabmap::Transform & transform);
+
+void RTABMAP_EXP fillRegisteredDepthHoles(cv::Mat & depth, bool vertical, bool horizontal, bool fillDoubleHoles = false);
+
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
 
-std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
-cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
-
-cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
-
-std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
-cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
-
-cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
-cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
-
 // remove depth by z axis
 void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
 									  const std::multimap<int, pcl::PointXYZ> & words2,
@@ -308,7 +292,7 @@ Transform RTABMAP_EXP icp(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -317,7 +301,7 @@ Transform RTABMAP_EXP icpPointToPlane(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 Transform RTABMAP_EXP icp2D(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -326,7 +310,7 @@ Transform RTABMAP_EXP icp2D(
 		int maximumIterations,
 		bool * hasConverged = 0,
 		double * variance = 0,
-		int * inliers = 0);
+		int * correspondences = 0);
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -374,61 +358,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		int samples,
 		const Transform & transform = Transform::getIdentity());
 
-inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
-{
-	Eigen::Matrix4f m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
-{
-	Eigen::Matrix4d m;
-	m << transform[0], transform[1], transform[2], transform[3],
-		 transform[4], transform[5], transform[6], transform[7],
-		 transform[8], transform[9], transform[10], transform[11],
-		 0,0,0,1;
-	return m;
-}
-
-inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
-{
-	return Eigen::Affine3f(transformToEigen4f(transform));
-}
-
-inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
-{
-	return Eigen::Affine3d(transformToEigen4d(transform));
-}
-
-inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
-inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
-{
-	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
-					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
-					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
@@ -449,63 +378,11 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumAngle = 2*M_PI/3,
 		bool gp3NormalConsistency = false);
 
-std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
+void RTABMAP_EXP occupancy2DFromLaserScan(
-		std::multimap<int, Link> & links,
+		const cv::Mat & scan,
-		int from,
-		int to);
-
-// <int, depth> depth=0 means infinite depth
-std::map<int, int> RTABMAP_EXP generateDepthGraph(
-		const std::multimap<int, Link> & links,
-		int fromId,
-		int depth = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, int> & depthGraph,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		bool ignoreCovariance = false,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-void RTABMAP_EXP optimizeTOROGraph(
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints,
-		std::map<int, Transform> & optimizedPoses,
-		int toroIterations = 100,
-		bool toroInitialGuess = true,
-		bool ignoreCovariance = false,
-		std::list<std::map<int, Transform> > * intermediateGraphes = 0);
-
-bool RTABMAP_EXP saveTOROGraph(
-		const std::string & fileName,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & edgeConstraints);
-
-bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
-		std::map<int, Transform> & poses,
-		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
-
-std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle,
-		bool keepLatest = true);
-
-std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
-		const std::map<int, Transform> & poses,
-		float radius,
-		float angle);
-
-bool RTABMAP_EXP occupancy2DFromCloud3D(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
-		float cellSize = 0.05f,
+		float cellSize);
-		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -513,8 +390,8 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		int fillEmptyRadius = 0,
+		float minMapSize = 0.0f,
-		float minMapSize = 0.0f);
+		bool erode = false);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
@@ -557,6 +434,13 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
 		const pcl::IndicesPtr & indicesA,
 		const pcl::IndicesPtr & indicesB);
 
+cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
+
+void RTABMAP_EXP savePCDWords(
+		const std::string & fileName,
+		const std::multimap<int, pcl::PointXYZ> & words,
+		const Transform & transform = Transform::getIdentity());
+
 ///////////////////
 // Templated PCL methods
 ///////////////////
@@ -710,6 +594,15 @@ pcl::IndicesPtr extractNegativeIndices(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices);
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize = 0.05f,
+		float groundNormalAngle = M_PI_4,
+		int minClusterSize = 20);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/BayesFilter.cpp

@@ -75,7 +75,7 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
 		bool valid = true;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			tmpValues[i] = std::atof((*iter).c_str());
+			tmpValues[i] = uStr2Float((*iter).c_str());
 			//UINFO("%d=%e", i, tmpValues[i]);
 			if(tmpValues[i] < 0.0 || tmpValues[i]>1.0)
 			{
@@ -273,7 +273,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 					{
 						if(iter->second == 0)
 						{
-							idsLoopMargin.push_back(iter->second);
+							idsLoopMargin.push_back(iter->first);
 						}
 						++iter;
 					}

corelib/src/CMakeLists.txt

@@ -14,6 +14,7 @@ SET(SRC_FILES
     Camera.cpp
     CameraThread.cpp
     CameraRGBD.cpp
+    CameraModel.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp
@@ -25,13 +26,24 @@ SET(SRC_FILES
 	Transform.cpp
 	
 	util3d.cpp
-	Odometry.cpp
 	SensorData.cpp
+	Graph.cpp
+	Compression.cpp
+	
+	Odometry.cpp
+	OdometryThread.cpp
+	OdometryBOW.cpp
+	OdometryOpticalFlow.cpp
+	OdometryMono.cpp
+	OdometryICP.cpp
 	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp
 	toro3d/treeoptimizer3.cpp
 	
+	toro3d/posegraph2.cpp
+	toro3d/treeoptimizer2.cpp
+	
 	sqlite3/sqlite3.c
 )
 
@@ -78,6 +90,54 @@ IF(OpenNI2_FOUND)
 	)
 ENDIF(OpenNI2_FOUND)
 
+IF(freenect2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FREENECT2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${freenect2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${freenect2_LIBRARIES}
+	)
+ENDIF(freenect2_FOUND)
+
+IF(DC1394_FOUND)
+	ADD_DEFINITIONS("-DWITH_DC1394")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${DC1394_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${DC1394_LIBRARIES}
+	)
+ENDIF(DC1394_FOUND)
+
+IF(FlyCapture2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${FlyCapture2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${FlyCapture2_LIBRARIES}
+	)
+ENDIF(FlyCapture2_FOUND)
+
+IF(G2O_FOUND)
+	ADD_DEFINITIONS("-DWITH_G2O")
+	SET(INCLUDE_DIRS 
+		${INCLUDE_DIRS} 
+		${G2O_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${G2O_LIBRARIES}
+	)
+ENDIF(G2O_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/Camera.cpp

@@ -50,6 +50,7 @@ Camera::Camera(float imageRate,
 	_imageRate(imageRate),
 	_imageWidth(imageWidth),
 	_imageHeight(imageHeight),
+	_mirroring(false),
 	_frameRateTimer(new UTimer())
 {
 }
@@ -74,6 +75,79 @@ void Camera::getImageSize(unsigned int & width, unsigned int & height)
 	height = _imageHeight;
 }
 
+void Camera::setCalibration(const std::string & fileName)
+{
+	if(UFile::getExtension(fileName).compare("yaml") == 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;
@@ -99,6 +173,15 @@ cv::Mat Camera::takeImage()
 
 	UTimer timer;
 	img = this->captureImage();
+	if(!img.empty() && !_k.empty() && !_d.empty())
+	{
+		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;
 }
@@ -213,6 +296,14 @@ cv::Mat CameraImages::captureImage()
 						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;
+					}
 				}
 			}
 		}

corelib/src/CameraModel.cpp

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

corelib/src/CameraThread.cpp

@@ -125,20 +125,17 @@ void CameraThread::mainLoop()
 	{
 		if(_cameraRGBD)
 		{
-			this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
+			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now());
+			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
 		}
 		else
 		{
-			this->post(new CameraEvent(rgb, ++_seq));
+			this->post(new CameraEvent(rgb, ++_seq, UTimer::now()));
 		}
 	}
 	else if(!this->isKilled())
 	{
 		if(_cameraRGBD)
-		{
-			UERROR("Retrieved data is empty! Stopping the camera...");
-		}
-		else
 		{
 			UWARN("no more images...");
 		}

corelib/src/Compression.cpp

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

corelib/src/DBDriver.cpp

@@ -38,7 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 DBDriver::DBDriver(const ParametersMap & parameters) :
-	_emptyTrashesTime(0)
+	_emptyTrashesTime(0),
+	_timestampUpdate(true)
 {
 	this->parseParameters(parameters);
 }
@@ -242,7 +243,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{
@@ -272,7 +273,7 @@ void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 
 		if(toUpdate.size())
 		{
-			this->updateQuery(toUpdate);
+			this->updateQuery(toUpdate, _timestampUpdate);
 		}
 		if(toSave.size())
 		{
@@ -306,8 +307,6 @@ void DBDriver::loadSignatures(const std::list<int> & signIds,
 	bool valueFound = false;
 	_trashesMutex.lock();
 	{
-		_dbSafeAccessMutex.lock();
-		_dbSafeAccessMutex.unlock();
 		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
 		{
 			valueFound = false;
@@ -360,8 +359,6 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	{
 		if(_trashVisualWords.size())
 		{
-			_dbSafeAccessMutex.lock();
-			_dbSafeAccessMutex.unlock();
 			for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
 			{
 				wIter = _trashVisualWords.find(*iter);
@@ -393,15 +390,26 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-//TODO Check also in the trash ?
 void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const
 {
+	// Don't look in the trash, we assume that if we want to load
+	// data of a signature, it is not in thrash! Print an error if so.
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			UASSERT(*iter != 0);
+			UASSERT_MSG(!uContains(_trashSignatures, (*iter)->id()), uFormat("Signature %d should not be used when transferred to trash!!!!", (*iter)->id()).c_str());
+		}
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->loadNodeDataQuery(signatures, loadMetricData);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getNodeData(
 		int signatureId,
 		cv::Mat & imageCompressed,
@@ -411,74 +419,284 @@ void DBDriver::getNodeData(
 		float & fy,
 		float & cx,
 		float & cy,
-		Transform & localTransform) const
+		Transform & localTransform,
+		int & laserScanMaxPts) const
 {
-	_dbSafeAccessMutex.lock();
+	bool found = false;
-	this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform);
+	// look in the trash
-	_dbSafeAccessMutex.unlock();
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		if(!s->getImageCompressed().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();
+			found = true;
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, laserScanCompressed, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
 {
-	_dbSafeAccessMutex.lock();
+	bool found = false;
-	this->getNodeDataQuery(signatureId, imageCompressed);
+	// look in the trash
-	_dbSafeAccessMutex.unlock();
+	_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);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
+bool DBDriver::getNodeInfo(int signatureId,
-void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
+		Transform & pose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp,
+		std::vector<unsigned char> & userData) const
 {
-	_dbSafeAccessMutex.lock();
+	bool found = false;
-	this->getPoseQuery(signatureId, pose, mapId);
+	// look in the trash
-	_dbSafeAccessMutex.unlock();
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		pose = _trashSignatures.at(signatureId)->getPose();
+		mapId = _trashSignatures.at(signatureId)->mapId();
+		weight = _trashSignatures.at(signatureId)->getWeight();
+		label = _trashSignatures.at(signatureId)->getLabel();
+		stamp = _trashSignatures.at(signatureId)->getStamp();
+		userData = _trashSignatures.at(signatureId)->getUserData();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
 }
 
-//TODO Check also in the trash ?
 void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
 {
-	_dbSafeAccessMutex.lock();
+	bool found = false;
-	this->loadLinksQuery(signatureId, links, type);
+	// look in the trash
-	_dbSafeAccessMutex.unlock();
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		const Signature * s = _trashSignatures.at(signatureId);
+		UASSERT(s != 0);
+		for(std::map<int, Link>::const_iterator nIter = s->getLinks().begin();
+				nIter!=s->getLinks().end();
+				++nIter)
+		{
+			if(type == Link::kUndef || nIter->second.type() == type)
+			{
+				links.insert(*nIter);
+			}
+		}
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->loadLinksQuery(signatureId, links, type);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getWeight(int signatureId, int & weight) const
 {
-	_dbSafeAccessMutex.lock();
+	bool found = false;
-	this->getWeightQuery(signatureId, weight);
+	// look in the trash
-	_dbSafeAccessMutex.unlock();
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		weight = _trashSignatures.at(signatureId)->getWeight();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->getWeightQuery(signatureId, weight);
+		_dbSafeAccessMutex.unlock();
+	}
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			bool hasNeighbors = !ignoreChildren;
+			if(ignoreChildren)
+			{
+				for(std::map<int, Link>::const_iterator nIter = sIter->second->getLinks().begin();
+						nIter!=sIter->second->getLinks().end();
+						++nIter)
+				{
+					if(nIter->second.type() == Link::kNeighbor)
+					{
+						hasNeighbors = true;
+						break;
+					}
+				}
+			}
+			if(hasNeighbors)
+			{
+				ids.insert(sIter->first);
+			}
+		}
+
+		std::vector<int> keys = uKeys(_trashSignatures);
+
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getAllNodeIdsQuery(ids, ignoreChildren);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getLastNodeId(int & id) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashSignatures.size())
+	{
+		id = _trashSignatures.rbegin()->first;
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getLastIdQuery("Node", id);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getLastWordId(int & id) const
 {
+	// look in the trash
+	_trashesMutex.lock();
+	if(_trashVisualWords.size())
+	{
+		id = _trashVisualWords.rbegin()->first;
+	}
+	_trashesMutex.unlock();
+
 	_dbSafeAccessMutex.lock();
 	this->getLastIdQuery("Word", id);
 	_dbSafeAccessMutex.unlock();
 }
 
-//TODO Check also in the trash ?
 void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
 {
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		ni = _trashSignatures.at(signatureId)->getWords().size();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		this->getInvertedIndexNiQuery(signatureId, ni);
+		_dbSafeAccessMutex.unlock();
+	}
+}
+
+void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
+{
+	if(!label.empty())
+	{
+		int idFound = 0;
+		// look in the trash
+		_trashesMutex.lock();
+		for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+		{
+			if(sIter->second->getLabel().compare(label) == 0)
+			{
+				idFound = sIter->first;
+				break;
+			}
+		}
+		_trashesMutex.unlock();
+
+		// then look in the database
+		if(idFound == 0)
+		{
+			_dbSafeAccessMutex.lock();
+			this->getNodeIdByLabelQuery(label, id);
+			_dbSafeAccessMutex.unlock();
+		}
+		else
+		{
+			id = idFound;
+		}
+	}
+	else
+	{
+		UWARN("Can't search with an empty label!");
+	}
+}
+
+void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
+{
+	// look in the trash
+	_trashesMutex.lock();
+	for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
+	{
+		if(!sIter->second->getLabel().empty())
+		{
+			labels.insert(std::make_pair(sIter->first, sIter->second->getLabel()));
+		}
+	}
+	_trashesMutex.unlock();
+
+	// then look in the database
 	_dbSafeAccessMutex.lock();
-	this->getInvertedIndexNiQuery(signatureId, ni);
+	this->getAllLabelsQuery(labels);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -340,7 +340,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	}
 	if(rc != SQLITE_OK)
 	{
-		UFATAL("DB error : %s", sqlite3_errmsg(_ppDb));
+		UFATAL("DB error : %s (path=\"%s\")", sqlite3_errmsg(_ppDb), url.c_str());
 		_ppDb = 0;
 		return false;
 	}
@@ -458,10 +458,20 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 
 		if(loadMetricData)
 		{
-			if(uStrNumCmp(_version, "0.7.0") < 0)
+			if(uStrNumCmp(_version, "0.8.11") >= 0)
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+					  << "FROM Image "
+					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+					  << "ON Image.id = Depth.id "
+					  << "WHERE Image.id = ?"
+					  <<";";
+			}
+			else if(uStrNumCmp(_version, "0.7.0") >= 0)
+			{
+				query << "SELECT Image.data, "
+						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -471,7 +481,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			else
 			{
 				query << "SELECT Image.data, "
-						 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+						 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
 					  << "FROM Image "
 					  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 					  << "ON Image.id = Depth.id "
@@ -553,14 +563,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					}
 					(*iter)->setLocalTransform(localTransform);
 
+					int laserScanMaxPts = 0;
+					if(uStrNumCmp(_version, "0.8.11") >= 0)
+					{
+						laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
+					}
+
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 					//Create the laserScan
 					if(dataSize>4 && data)
 					{
-						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
+						(*iter)->setLaserScanCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(), laserScanMaxPts); // depth2d
 					}
-
 				}
 
 				rc = sqlite3_step(ppStmt); // next result...
@@ -588,7 +603,8 @@ void DBDriverSqlite3::getNodeDataQuery(
 		float & fy,
 		float & cx,
 		float & cy,
-		Transform & localTransform) const
+		Transform & localTransform,
+		int & laserScanMaxPts) const
 {
 	if(_ppDb)
 	{
@@ -598,10 +614,20 @@ void DBDriverSqlite3::getNodeDataQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.7.0") < 0)
+		if(uStrNumCmp(_version, "0.8.11") >= 0)
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d_max_pts, Depth.data2d "
+				  << "FROM Image "
+				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
+				  << "ON Image.id = Depth.id "
+				  << "WHERE Image.id = " << signatureId
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.7.0") >= 0)
+		{
+			query << "SELECT Image.data, "
+					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -611,7 +637,7 @@ void DBDriverSqlite3::getNodeDataQuery(
 		else
 		{
 			query << "SELECT Image.data, "
-					 "Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
+					 "Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
 				  << "FROM Image "
 				  << "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
 				  << "ON Image.id = Depth.id "
@@ -675,6 +701,12 @@ void DBDriverSqlite3::getNodeDataQuery(
 				memcpy(localTransform.data(), data, dataSize);
 			}
 
+			laserScanMaxPts = 0;
+			if(uStrNumCmp(_version, "0.8.11") >= 0)
+			{
+				laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
+			}
+
 			data = sqlite3_column_blob(ppStmt, index); // depth2d
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
 			//Create the depth2d
@@ -751,8 +783,15 @@ void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompresse
 	}
 }
 
-void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapId) const
+bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
+		Transform & pose,
+		int & mapId,
+		int & weight,
+		std::string & label,
+		double & stamp,
+		std::vector<unsigned char> & userData) const
 {
+	bool found = false;
 	if(_ppDb && signatureId)
 	{
 		int rc = SQLITE_OK;
@@ -760,10 +799,27 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		std::stringstream query;
 
 		// Prepare the query... Get the map from signature and visual words
-		query << "SELECT pose, map_id "
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
-				 "FROM Node "
+		{
-				 "WHERE id = " << signatureId <<
+			query << "SELECT pose, map_id, weight, label, stamp, user_data "
-				 ";";
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			query << "SELECT pose, map_id, weight, label, stamp "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
+		else
+		{
+			query << "SELECT pose, map_id, weight "
+					 "FROM Node "
+					 "WHERE id = " << signatureId <<
+					 ";";
+		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -775,6 +831,7 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		rc = sqlite3_step(ppStmt);
 		if(rc == SQLITE_ROW)
 		{
+			found = true;
 			int index = 0;
 			data = sqlite3_column_blob(ppStmt, index); // pose
 			dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -784,7 +841,29 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 			}
 
 			mapId = sqlite3_column_int(ppStmt, index++); // map id
+			weight = sqlite3_column_int(ppStmt, index++); // weight
 
+			if(uStrNumCmp(_version, "0.8.5") >= 0)
+			{
+				const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+				if(p)
+				{
+					label = reinterpret_cast<const char*>(p); // label
+				}
+				stamp = sqlite3_column_double(ppStmt, index++); // stamp
+			}
+
+			if(uStrNumCmp(_version, "0.8.8") >= 0)
+			{
+				data = sqlite3_column_blob(ppStmt, index);
+				dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
+
+				if(dataSize && data)
+				{
+					userData.resize(dataSize);
+					memcpy(userData.data(), data, dataSize);
+				}
+			}
 
 			rc = sqlite3_step(ppStmt); // next result...
 		}
@@ -794,6 +873,7 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
+	return found;
 }
 
 
@@ -817,9 +897,8 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 		{
 			query << "SELECT id "
 				  << "FROM Node "
-				  << "LEFT OUTER JOIN Link "
+				  << "INNER JOIN Link "
-				  << "ON id = from_id "
+				  << "ON id = to_id " // use to_id tp ignore all children (which don't have link pointing on them)
-				  << "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
 				  << "ORDER BY id";
 		}
 
@@ -927,6 +1006,76 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
 	}
 }
 
+void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
+{
+	if(_ppDb && !label.empty() && uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+		query << "SELECT id FROM Node WHERE label='" << label <<"'";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		if(rc == SQLITE_ROW)
+		{
+			id = sqlite3_column_int(ppStmt, 0);
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%f", timer.ticks());
+	}
+}
+
+void DBDriverSqlite3::getAllLabelsQuery(std::map<int, std::string> & labels) const
+{
+	if(_ppDb && uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+		query << "SELECT id,label FROM Node WHERE label IS NOT NULL";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		while(rc == SQLITE_ROW)
+		{
+			int index = 0;
+			int id = sqlite3_column_int(ppStmt, index++);
+			const unsigned char * p = sqlite3_column_text(ppStmt, index++);
+			if(p)
+			{
+				std::string label = reinterpret_cast<const char*>(p);
+				if(!label.empty())
+				{
+					labels.insert(std::make_pair(id, label));
+				}
+			}
+			rc = sqlite3_step(ppStmt);
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%f", timer.ticks());
+	}
+}
+
 void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
 {
 	weight = 0;
@@ -976,9 +1125,24 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 		unsigned int loaded = 0;
 
 		// Load nodes information
-		query << "SELECT id, map_id, weight, pose "
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
-			  << "FROM Node "
+		{
-			  << "WHERE id=?;";
+			query << "SELECT id, map_id, weight, pose, stamp, label, user_data "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			query << "SELECT id, map_id, weight, pose, stamp, label "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
+		else
+		{
+			query << "SELECT id, map_id, weight, pose "
+				  << "FROM Node "
+				  << "WHERE id=?;";
+		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -992,10 +1156,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			int id = 0;
 			int mapId = 0;
+			double stamp = 0.0;
 			int weight = 0;
 			Transform pose;
 			const void * data = 0;
 			int dataSize = 0;
+			std::string label;
+			std::vector<unsigned char> userData;
 
 			// Process the result if one
 			rc = sqlite3_step(ppStmt);
@@ -1012,6 +1179,29 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				{
 					memcpy(pose.data(), data, dataSize);
 				}
+
+				if(uStrNumCmp(_version, "0.8.5") >= 0)
+				{
+					stamp = sqlite3_column_double(ppStmt, index++); // stamp
+					const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
+					if(p)
+					{
+						label = reinterpret_cast<const char*>(p);
+					}
+				}
+
+				if(uStrNumCmp(_version, "0.8.8") >= 0)
+				{
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
+
+					if(dataSize && data)
+					{
+						userData.resize(dataSize);
+						memcpy(userData.data(), data, dataSize);
+					}
+				}
+
 				rc = sqlite3_step(ppStmt);
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1023,10 +1213,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				Signature * s = new Signature(
 						id,
 						mapId,
+						weight,
+						stamp,
+						label,
 						std::multimap<int, cv::KeyPoint>(),
 						std::multimap<int, pcl::PointXYZ>(),
-						pose);
+						pose,
-				s->setWeight(weight);
+						userData);
 				s->setSaved(true);
 				nodes.push_back(s);
 				++loaded;
@@ -1094,7 +1287,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			if(visualWords.size()==0)
 			{
-				UWARN("Empty signature detected! (id=%d)", (*iter)->id());
+				UDEBUG("Empty signature detected! (id=%d)", (*iter)->id());
 			}
 			else
 			{
@@ -1360,7 +1553,11 @@ void DBDriverSqlite3::loadLinksQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.7.4") >= 0)
+		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		{
+			query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
+		}
+		else if(uStrNumCmp(_version, "0.7.4") >= 0)
 		{
 			query << "SELECT to_id, type, transform, variance FROM Link ";
 		}
@@ -1391,7 +1588,8 @@ void DBDriverSqlite3::loadLinksQuery(
 
 		int toId = -1;
 		int type = Link::kUndef;
-		float variance = 1.0f;
+		float rotVariance = 1.0f;
+		float transVariance = 1.0f;
 		const void * data = 0;
 		int dataSize = 0;
 
@@ -1417,15 +1615,21 @@ void DBDriverSqlite3::loadLinksQuery(
 				UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
 			}
 
-			if(uStrNumCmp(_version, "0.7.4") >= 0)
+			if(uStrNumCmp(_version, "0.8.4") >= 0)
 			{
-				variance = sqlite3_column_double(ppStmt, index++);
+				rotVariance = sqlite3_column_double(ppStmt, index++);
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
+				transVariance = sqlite3_column_double(ppStmt, index++);
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+			}
+			else if(uStrNumCmp(_version, "0.7.4") >= 0)
+			{
+				rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
 			}
 			else
 			{
 				// neighbor is 0, loop closures are 1 and 2 (child)
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, variance)));
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
 			}
 
 			rc = sqlite3_step(ppStmt);
@@ -1455,7 +1659,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;
 
-		if(uStrNumCmp(_version, "0.7.4") >= 0)
+		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		{
+			query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else if(uStrNumCmp(_version, "0.7.4") >= 0)
 		{
 			query << "SELECT to_id, type, variance, transform FROM Link "
 				  << "WHERE from_id = ? "
@@ -1479,7 +1689,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 			int toId = -1;
 			int linkType = -1;
-			float variance = 1.0f;
+			float rotVariance = 1.0f;
+			float transVariance = 1.0f;
 			std::list<Link> links;
 			const void * data = 0;
 			int dataSize = 0;
@@ -1492,9 +1703,14 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
-				if(uStrNumCmp(_version, "0.7.4") >= 0)
+				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
-					variance = sqlite3_column_double(ppStmt, index++);
+					rotVariance = sqlite3_column_double(ppStmt, index++);
+					transVariance = sqlite3_column_double(ppStmt, index++);
+				}
+				else if(uStrNumCmp(_version, "0.7.4") >= 0)
+				{
+					rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
 				}
 
 				//transform
@@ -1514,11 +1730,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				{
 					if(uStrNumCmp(_version, "0.7.4") >= 0)
 					{
-						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
 					}
 					else // neighbor is 0, loop closures are 1 and 2 (child)
 					{
-						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, variance));
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
 					}
 				}
 				else
@@ -1548,7 +1764,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 }
 
 
-void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
+void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
 {
 	UDEBUG("nodes = %d", nodes.size());
 	if(_ppDb && nodes.size())
@@ -1559,7 +1775,40 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 		sqlite3_stmt * ppStmt = 0;
 		Signature * s = 0;
 
-		std::string query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+		std::string query;
+		if(uStrNumCmp(_version, "0.8.8") >= 0)
+		{
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, label=?, user_data=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=?, label=?, user_data=? WHERE id=?;";
+			}
+		}
+		else if(uStrNumCmp(_version, "0.8.5") >= 0)
+		{
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
+			}
+		}
+		else
+		{
+			if(updateTimestamp)
+			{
+				query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
+			}
+			else
+			{
+				query = "UPDATE Node SET weight=? WHERE id=?;";
+			}
+		}
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -1572,6 +1821,34 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 				rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+				if(uStrNumCmp(_version, "0.8.5") >= 0)
+				{
+					if(s->getLabel().empty())
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+					else
+					{
+						rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+				}
+
+				if(uStrNumCmp(_version, "0.8.8") >= 0)
+				{
+					if(s->getUserData().empty())
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+					else
+					{
+						rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+					}
+				}
+
 				rc = sqlite3_bind_int(ppStmt, index++, s->id());
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
@@ -1623,7 +1900,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 				const std::map<int, Link> & links = (*j)->getLinks();
 				for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 				{
-					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
+					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
 				}
 			}
 		}
@@ -1655,10 +1932,11 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes) const
 	}
 }
 
-void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words) const
+void DBDriverSqlite3::updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const
 {
-	if(_ppDb && words.size())
+	if(_ppDb && words.size() && updateTimestamp)
 	{
+		// Only timestamp update is done here, so don't enter this if at all if false
 		UTimer timer;
 		timer.start();
 		int rc = SQLITE_OK;
@@ -1730,7 +2008,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			const std::map<int, Link> & links = (*jter)->getLinks();
 			for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 			{
-				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
+				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
 			}
 		}
 		// Finalize (delete) the statement
@@ -1798,7 +2076,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			//metric
 			if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
 			{
-				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
+				stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform(), (*i)->getLaserScanMaxPts());
 			}
 		}
 		// Finalize (delete) the statement
@@ -1868,6 +2146,14 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 
 std::string DBDriverSqlite3::queryStepNode() const
 {
+	if(uStrNumCmp(_version, "0.8.8") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
+	}
 	return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
 }
 void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
@@ -1889,6 +2175,37 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 	rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.8.5") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+
+		if(s->getLabel().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
+	if(uStrNumCmp(_version, "0.8.8") >= 0)
+	{
+		if(s->getUserData().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1937,13 +2254,17 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepDepth() const
 {
-	if(uStrNumCmp(_version, "0.7.0") < 0)
+	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
-		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d, data2d_max_pts) VALUES(?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.7.0") >= 0)
+	{
+		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
 	}
 	else
 	{
-		return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
+		return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
 	}
 }
 void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
@@ -1954,7 +2275,8 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		float fy,
 		float cx,
 		float cy,
-		const Transform & localTransform) const
+		const Transform & localTransform,
+		int depth2dMaxPts) const
 {
 	UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
 	if(!ppStmt)
@@ -1978,12 +2300,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(uStrNumCmp(_version, "0.7.0") < 0)
+	if(uStrNumCmp(_version, "0.7.0") >= 0)
-	{
-		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
-		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-	}
-	else
 	{
 		rc = sqlite3_bind_double(ppStmt, index++, fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1994,6 +2311,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 		rc = sqlite3_bind_double(ppStmt, index++, cy);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
+	else
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
 
 	rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2008,6 +2330,12 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
+	if(uStrNumCmp(_version, "0.8.11") >= 0)
+	{
+		rc = sqlite3_bind_int(ppStmt, index++, depth2dMaxPts);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2018,7 +2346,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepLink() const
 {
-	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	if(uStrNumCmp(_version, "0.8.4") >= 0)
+	{
+		return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.7.4") >= 0)
 	{
 		return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
 	}
@@ -2027,13 +2359,28 @@ std::string DBDriverSqlite3::queryStepLink() const
 		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const
+void DBDriverSqlite3::stepLink(
+		sqlite3_stmt * ppStmt,
+		int fromId,
+		int toId,
+		Link::Type type,
+		float rotVariance,
+		float transVariance,
+		const Transform & transform) const
 {
 	if(!ppStmt)
 	{
 		UFATAL("");
 	}
 	UDEBUG("Save link from %d to %d, type=%d", fromId, toId, type);
+
+	// Don't save virtual links
+	if(type==Link::kVirtualClosure)
+	{
+		UDEBUG("Virtual link ignored....");
+		return;
+	}
+
 	int rc = SQLITE_OK;
 	int index = 1;
 	rc = sqlite3_bind_int(ppStmt, index++, fromId);
@@ -2043,9 +2390,16 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int
 	rc = sqlite3_bind_int(ppStmt, index++, type);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, variance);
+		rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, transVariance);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else if(uStrNumCmp(_version, "0.7.4") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 

corelib/src/DBDriverSqlite3.h

@@ -60,8 +60,8 @@ private:
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
-	virtual void updateQuery(const std::list<Signature *> & signatures) const;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
-	virtual void updateQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
@@ -80,12 +80,15 @@ private:
 			float & fy,
 			float & cx,
 			float & cy,
-			Transform & localTransform) const;
+			Transform & localTransform,
+			int & laserScanMaxPts) const;
 	virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
-	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) 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 getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
 private:
 	std::string queryStepNode() const;
@@ -108,8 +111,9 @@ private:
 			float fy,
 			float cx,
 			float cy,
-			const Transform & localTransform) const;
+			const Transform & localTransform,
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, float variance, const Transform & transform) const;
+			int depth2dMaxPts) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) 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;
 

corelib/src/DBReader.cpp

@@ -31,25 +31,41 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 
 #include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 
 namespace rtabmap {
 
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
-				   float delayToStartSec) :
+				   bool ignoreGoalDelay) :
-	_path(databasePath),
+	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
-	_delayToStartSec(delayToStartSec),
+	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
+}
 
+DBReader::DBReader(const std::list<std::string> & databasePaths,
+				   float frameRate,
+				   bool odometryIgnored,
+				   bool ignoreGoalDelay) :
+   _paths(databasePaths),
+   _frameRate(frameRate),
+	_odometryIgnored(odometryIgnored),
+	_ignoreGoalDelay(ignoreGoalDelay),
+	_dbDriver(0),
+	_currentId(_ids.end())
+{
 }
 
 DBReader::~DBReader()
@@ -71,10 +87,18 @@ bool DBReader::init(int startIndex)
 	}
 	_ids.clear();
 	_currentId=_ids.end();
+	_previousStamp = 0;
 
-	if(!UFile::exists(_path))
+	if(_paths.size() == 0)
 	{
-		UERROR("Database path does not exist (%s)", _path.c_str());
+		UERROR("No database path set...");
+		return false;
+	}
+
+	std::string path = _paths.front();
+	if(!UFile::exists(path))
+	{
+		UERROR("Database path does not exist (%s)", path.c_str());
 		return false;
 	}
 
@@ -86,9 +110,9 @@ bool DBReader::init(int startIndex)
 		UERROR("Driver doesn't exist.");
 		return false;
 	}
-	if(!_dbDriver->openConnection(_path))
+	if(!_dbDriver->openConnection(path))
 	{
-		UERROR("Can't open database %s", _path.c_str());
+		UERROR("Can't open database %s", path.c_str());
 		delete _dbDriver;
 		_dbDriver = 0;
 		return false;
@@ -98,10 +122,10 @@ bool DBReader::init(int startIndex)
 	_currentId = _ids.begin();
 	if(startIndex>0 && _ids.size())
 	{
-		std::set<int>::iterator iter = _ids.lower_bound(startIndex);
+		std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
 		if(iter == _ids.end())
 		{
-			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, *_ids.rbegin());
+			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
 		}
 		else
 		{
@@ -114,18 +138,11 @@ bool DBReader::init(int startIndex)
 
 void DBReader::setFrameRate(float frameRate)
 {
-	if(frameRate >= 0.0f)
+	_frameRate = frameRate;
-	{
-		_frameRate = frameRate;
-	}
 }
 
 void DBReader::mainLoopBegin()
 {
-	if(_delayToStartSec > 0.0f)
-	{
-		uSleep(_delayToStartSec*1000.0f);
-	}
 	_timer.start();
 }
 
@@ -134,6 +151,23 @@ void DBReader::mainLoop()
 	SensorData data = this->getNextData();
 	if(data.isValid())
 	{
+		int goalId = 0;
+		double previousStamp = data.stamp();
+		data.setStamp(UTimer::now());
+		if(data.userData().size() >= 6 && memcmp(data.userData().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());
+			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>());
+				}
+			}
+		}
 		if(!_odometryIgnored)
 		{
 			if(data.pose().isNull())
@@ -149,12 +183,56 @@ void DBReader::mainLoop()
 			this->post(new CameraEvent(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
+				// that was used originally for planning
+				int weight;
+				std::string label;
+				double stamp;
+				int mapId;
+				Transform localTransform, pose;
+				std::vector<unsigned char> userData;
+				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+				if(previousStamp && stamp && stamp > previousStamp)
+				{
+					double delay = stamp - previousStamp;
+					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
+							goalId, delay);
+					uSleep(delay*1000);
+				}
+			}
+			else
+			{
+				UWARN("Goal %d detected, posting it!", goalId);
+			}
+		}
+
 	}
 	else if(!this->isKilled())
 	{
 		UINFO("no more images...");
-		this->kill();
+		if(_paths.size() > 1)
-		this->post(new CameraEvent());
+		{
+			_paths.pop_front();
+			UWARN("Loading next database \"%s\"...", _paths.front().c_str());
+			if(!this->init())
+			{
+				UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
+				this->kill();
+				this->post(new CameraEvent());
+			}
+		}
+		else
+		{
+			this->kill();
+			this->post(new CameraEvent());
+		}
+
 	}
 
 }
@@ -164,26 +242,6 @@ SensorData DBReader::getNextData()
 	SensorData data;
 	if(_dbDriver)
 	{
-		float frameRate = _frameRate;
-		if(frameRate>0.0f)
-		{
-			int sleepTime = (1000.0f/frameRate - 1000.0f*_timer.getElapsedTime());
-			if(sleepTime > 2)
-			{
-				uSleep(sleepTime-2);
-			}
-
-			// Add precision at the cost of a small overhead
-			while(_timer.getElapsedTime() < 1.0/double(frameRate)-0.000001)
-			{
-				//
-			}
-
-			double slept = _timer.getElapsedTime();
-			_timer.start();
-			UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
-		}
-
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
 			cv::Mat imageBytes;
@@ -192,19 +250,34 @@ SensorData DBReader::getNextData()
 			int mapId;
 			float fx,fy,cx,cy;
 			Transform localTransform, pose;
-			float variance = 1.0f;
+			float rotVariance = 1.0f;
-			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
+			float transVariance = 1.0f;
+			std::vector<unsigned char> userData;
+			int laserScanMaxPts = 0;
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform, laserScanMaxPts);
+
+			// info
+			int weight;
+			std::string label;
+			double stamp;
+			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
+
 			if(!_odometryIgnored)
 			{
-				_dbDriver->getPose(*_currentId, pose, mapId);
 				std::map<int, Link> links;
 				_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
 				if(links.size())
 				{
 					// assume the first is the backward neighbor, take its variance
-					variance = links.begin()->second.variance();
+					rotVariance = links.begin()->second.rotVariance();
+					transVariance = links.begin()->second.transVariance();
 				}
 			}
+			else
+			{
+				pose.setNull();
+			}
+
 			int seq = *_currentId;
 			++_currentId;
 			if(imageBytes.empty())
@@ -212,29 +285,83 @@ SensorData DBReader::getNextData()
 				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
 
-			util3d::CompressionThread ctImage(imageBytes, true);
+			// Frame rate
-			util3d::CompressionThread ctDepth(depthBytes, true);
+			if(_frameRate < 0.0f)
-			util3d::CompressionThread ctLaserScan(laserScanBytes, false);
+			{
-			ctImage.start();
+				if(stamp == 0)
-			ctDepth.start();
+				{
-			ctLaserScan.start();
+					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
-			ctImage.join();
+					this->kill();
-			ctDepth.join();
+				}
-			ctLaserScan.join();
+				else if(_previousStamp > 0)
-			data = SensorData(
+				{
-					ctLaserScan.getUncompressedData(),
+					int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime();
-					ctImage.getUncompressedData(),
+					if(sleepTime > 2)
-					ctDepth.getUncompressedData(),
+					{
-					fx,fy,cx,cy,
+						uSleep(sleepTime-2);
-					localTransform,
+					}
-					pose,
+
-					variance,
+					// Add precision at the cost of a small overhead
-					seq);
+					while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001)
-			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);
+					double slept = _timer.getElapsedTime();
+					_timer.start();
+					UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp);
+				}
+				_previousStamp = stamp;
+			}
+			else if(_frameRate>0.0f)
+			{
+				int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
+				if(sleepTime > 2)
+				{
+					uSleep(sleepTime-2);
+				}
+
+				// Add precision at the cost of a small overhead
+				while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
+				{
+					//
+				}
+
+				double slept = _timer.getElapsedTime();
+				_timer.start();
+				UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
+			}
+
+			if(!this->isKilled())
+			{
+				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);
+			}
 		}
 	}
 	else

corelib/src/EpipolarGeometry.cpp

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

corelib/src/Features2d.cpp

@@ -251,7 +251,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRat
 		unsigned int i=0;
 		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
 		{
-			values[i] = std::atof((*iter).c_str());
+			values[i] = uStr2Float(*iter);
 			++i;
 		}
 
@@ -319,6 +319,15 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 /////////////////////
 // Feature2D
 /////////////////////
+Feature2D::Feature2D(const ParametersMap & parameters) :
+		maxFeatures_(Parameters::defaultKpWordsPerImage())
+{
+	this->parseParameters(parameters);
+}
+void Feature2D::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
+}
 Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
 {
 	if(RTABMAP_NONFREE == 0 &&
@@ -369,9 +378,8 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 	}
 	return feature2D;
 }
-std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
+std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
 {
-	ULOGGER_DEBUG("");
 	std::vector<cv::KeyPoint> keypoints;
 	if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
 	{
@@ -381,7 +389,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, in
 		keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
 		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
 
-		limitKeypoints(keypoints, maxKeypoints);
+		limitKeypoints(keypoints, maxFeatures_);
 
 		if(roi.x || roi.y)
 		{
@@ -447,6 +455,8 @@ SURF::~SURF()
 
 void SURF::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
 	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
 	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
@@ -567,6 +577,8 @@ SIFT::~SIFT()
 
 void SIFT::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
 	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
@@ -615,7 +627,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 //ORB
 //////////////////////////
 ORB::ORB(const ParametersMap & parameters) :
-		nFeatures_(Parameters::defaultORBNFeatures()),
+		nFeatures_(Parameters::defaultKpWordsPerImage()),
 		scaleFactor_(Parameters::defaultORBScaleFactor()),
 		nLevels_(Parameters::defaultORBNLevels()),
 		edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
@@ -646,7 +658,9 @@ ORB::~ORB()
 
 void ORB::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures_);
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
 	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
 	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
 	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
@@ -764,6 +778,8 @@ FAST::~FAST()
 
 void FAST::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
 	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
 	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
@@ -903,7 +919,7 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 //GFTT
 //////////////////////////
 GFTT::GFTT(const ParametersMap & parameters) :
-		_maxCorners(Parameters::defaultGFTTMaxCorners()),
+		_maxCorners(Parameters::defaultKpWordsPerImage()),
 		_qualityLevel(Parameters::defaultGFTTQualityLevel()),
 		_minDistance(Parameters::defaultGFTTMinDistance()),
 		_blockSize(Parameters::defaultGFTTBlockSize()),
@@ -924,7 +940,9 @@ GFTT::~GFTT()
 
 void GFTT::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kGFTTMaxCorners(), _maxCorners);
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
 	Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
 	Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
 	Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
@@ -1058,6 +1076,8 @@ BRISK::~BRISK()
 
 void BRISK::parseParameters(const ParametersMap & parameters)
 {
+	Feature2D::parseParameters(parameters);
+
 	Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
 	Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
 	Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);

corelib/src/OdometryBOW.cpp

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

corelib/src/OdometryICP.cpp

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

corelib/src/OdometryMono.cpp

@@ -0,0 +1,930 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/EpipolarGeometry.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/video/tracking.hpp>
+
+namespace rtabmap {
+
+OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
+	Odometry(parameters),
+	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+	flowIterations_(Parameters::defaultOdomFlowIterations()),
+	flowEps_(Parameters::defaultOdomFlowEps()),
+	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
+	initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
+	initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
+	minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
+	fundMatrixReprojError_(Parameters::defaultVhEpRansacParam1()),
+	fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
+	maxVariance_(Parameters::defaultOdomMonoMaxVariance())
+{
+	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
+
+	Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
+	Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
+	Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
+	Parameters::parse(parameters, Parameters::kOdomMonoMaxVariance(), maxVariance_);
+
+	Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), fundMatrixReprojError_);
+	Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
+
+	// Setup memory
+	ParametersMap customParameters;
+	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
+	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
+	int nn = Parameters::defaultOdomBowNNType();
+	float nndr = Parameters::defaultOdomBowNNDR();
+	int featureType = Parameters::defaultOdomFeatureType();
+	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
+	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
+
+	int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
+	int subPixIterations = Parameters::defaultOdomSubPixIterations();
+	double subPixEps = Parameters::defaultOdomSubPixEps();
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
+	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
+
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	memory_ = new Memory(customParameters);
+	if(!memory_->init("", false, ParametersMap()))
+	{
+		UERROR("Error initializing the memory for Mono Odometry.");
+	}
+}
+
+OdometryMono::~OdometryMono()
+{
+	delete memory_;
+}
+
+void OdometryMono::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+	memory_->init("", false, ParametersMap());
+	localMap_.clear();
+	refDepth_ = cv::Mat();
+	cornersMap_.clear();
+	keyFrameWords3D_.clear();
+	keyFramePoses_.clear();
+}
+
+Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
+{
+	UASSERT(!data.image().empty());
+	UASSERT(data.fx());
+
+	UTimer timer;
+	Transform output;
+
+	int inliers = 0;
+	int correspondences = 0;
+	int nFeatures = 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();
+	}
+
+	if(memory_->getStMem().size() >= 1)
+	{
+		if(localMap_.size())
+		{
+			//PnP
+			UDEBUG("PnP");
+
+			if(this->isInfoDataFilled() && info)
+			{
+				info->type = 0;
+			}
+
+			// generate kpts
+			if(memory_->update(SensorData(newFrame)))
+			{
+				UDEBUG("");
+				bool newPtsAdded = false;
+				const Signature * newS = memory_->getLastWorkingSignature();
+				UDEBUG("newWords=%d", (int)newS->getWords().size());
+				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 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;
+					std::vector<cv::Point2f> imagePoints;
+					std::vector<int> matches;
+
+					UDEBUG("compute PnP from optical flow");
+
+					std::vector<int> ids = uKeys(localMap_);
+					objectPoints = uValues(localMap_);
+
+					// compute last projection
+					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();
+					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(),
+							(double)prevGuess.r31(), (double)prevGuess.r32(), (double)prevGuess.r33());
+					cv::Mat prevRvec(1,3, CV_64FC1);
+					cv::Rodrigues(prevR, prevRvec);
+					cv::Mat prevTvec = (cv::Mat_<double>(1,3) << (double)prevGuess.x(), (double)prevGuess.y(), (double)prevGuess.z());
+					cv::projectPoints(objectPoints, prevRvec, prevTvec, K, cv::Mat(), prevImagePoints);
+
+					// compute current projection
+					UDEBUG("project points to previous image");
+					cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
+
+					//filter points not in the image and set guess from unique correspondences
+					std::vector<cv::Point3f> objectPointsTmp(objectPoints.size());
+					std::vector<cv::Point2f> refCorners(objectPoints.size());
+					std::vector<cv::Point2f> newCorners(objectPoints.size());
+					matches.resize(objectPoints.size());
+					int oi=0;
+					for(unsigned int i=0; i<objectPoints.size(); ++i)
+					{
+						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))
+						{
+							refCorners[oi] = prevImagePoints[i];
+							newCorners[oi] = imagePoints[i];
+							if(localMap_.count(ids[i]) == 1)
+							{
+								if(prevS->getWords().count(ids[i]) == 1)
+								{
+									// set guess if unique
+									refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
+								}
+								if(newS->getWords().count(ids[i]) == 1)
+								{
+									// set guess if unique
+									newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
+								}
+							}
+							objectPointsTmp[oi] = objectPoints[i];
+							matches[oi] = ids[i];
+							++oi;
+						}
+					}
+					objectPointsTmp.resize(oi);
+					refCorners.resize(oi);
+					newCorners.resize(oi);
+					matches.resize(oi);
+
+					// Refine imagePoints using optical flow
+					std::vector<unsigned char> statusFlowInliers;
+					std::vector<float> err;
+					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+					cv::calcOpticalFlowPyrLK(
+							prevS->getImageRaw(),
+							newFrame,
+							refCorners,
+							newCorners,
+							statusFlowInliers,
+							err,
+							cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+							cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+							cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
+					UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+					objectPoints.resize(statusFlowInliers.size());
+					imagePoints.resize(statusFlowInliers.size());
+					std::vector<int> matchesTmp(statusFlowInliers.size());
+					oi = 0;
+					for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
+					{
+						if(statusFlowInliers[i])
+						{
+							objectPoints[oi] = objectPointsTmp[i];
+							imagePoints[oi] = newCorners[i];
+							matchesTmp[oi] = matches[i];
+							++oi;
+
+							if(this->isInfoDataFilled() && info)
+							{
+								cv::KeyPoint kpt;
+								if(newS->getWords().count(matches[i]) == 1)
+								{
+									kpt = newS->getWords().find(matches[i])->second;
+								}
+								kpt.pt = newCorners[i];
+								info->words.insert(std::make_pair(matches[i], kpt));
+							}
+						}
+					}
+					UDEBUG("Flow inliers= %d/%d", oi, (int)statusFlowInliers.size());
+					objectPoints.resize(oi);
+					imagePoints.resize(oi);
+					matchesTmp.resize(oi);
+					matches = matchesTmp;
+
+					if(this->isInfoDataFilled() && info)
+					{
+						info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
+					}
+					correspondences = (int)matches.size();
+
+					if((int)matches.size() < this->getMinInliers())
+					{
+						UWARN("not enough matches (%d < %d)...", (int)matches.size(), this->getMinInliers());
+					}
+					else
+					{
+						//PnPRansac
+						std::vector<int> inliersV;
+						cv::solvePnPRansac(
+								objectPoints,
+								imagePoints,
+								K,
+								cv::Mat(),
+								rvec,
+								tvec,
+								true,
+								this->getIterations(),
+								this->getPnPReprojError(),
+								0,
+								inliersV,
+								this->getPnPFlags());
+
+						UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
+
+						inliers = (int)inliersV.size();
+						if((int)inliersV.size() < this->getMinInliers())
+						{
+							UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+						}
+						else
+						{
+							cv::Mat R(3,3,CV_64FC1);
+							cv::Rodrigues(rvec, R);
+							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();
+
+							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]]; // index and ID should match (index starts at 0, ID starts at 1)
+								}
+							}
+
+							//Find the frame with the most similar features
+							std::set<int> stMem =  memory_->getStMem();
+							stMem.erase(newS->id());
+							std::map<int, float> likelihood = memory_->computeLikelihood(newS, std::list<int>(stMem.begin(), stMem.end()));
+							int maxLikelihoodId = -1;
+							float maxLikelihood = 0;
+							for(std::map<int, float>::iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
+							{
+								if(iter->second > maxLikelihood)
+								{
+									maxLikelihood = iter->second;
+									maxLikelihoodId = iter->first;
+								}
+							}
+							UASSERT(maxLikelihoodId != -1);
+
+							// Add new points to local map
+							const Signature* previousS = memory_->getSignature(maxLikelihoodId);
+							UASSERT(previousS!=0);
+							Transform cameraTransform = keyFramePoses_.at(previousS->id()).inverse()*this->getPose()*output;
+							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",
+										cameraTransform.getNorm(), minTranslation_);
+							}
+							else
+							{
+
+								double variance = 0;
+								const std::multimap<int, pcl::PointXYZ> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
+								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(),
+										cameraTransform,
+										this->getIterations(),
+										this->getPnPReprojError(),
+										this->getPnPFlags(),
+										fundMatrixReprojError_,
+										fundMatrixConfidence_,
+										previousGuess,
+										&variance);
+
+								if((int)inliers3D.size() < this->getMinInliers())
+								{
+									UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
+									(int)inliers3D.size(), this->getMinInliers(), cameraTransform.prettyPrint().c_str());
+								}
+								else if(variance == 0 || variance > maxVariance_)
+								{
+									UWARN("Variance too high %f (max = %f)", variance, maxVariance_);
+								}
+								else
+								{
+									UDEBUG("inliers3D=%d/%d variance=  %f", inliers3D.size(), newS->getWords().size(), variance);
+									Transform newPose = keyFramePoses_.at(previousS->id())*cameraTransform;
+									UDEBUG("cameraTransform=  %s", cameraTransform.prettyPrint().c_str());
+
+									std::multimap<int, cv::Point3f> wordsToAdd;
+									for(std::multimap<int, pcl::PointXYZ>::iterator iter=inliers3D.begin();
+										iter != inliers3D.end();
+										++iter)
+									{
+										// transform inliers3D in new signature referential
+										iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
+
+										if(!uContains(localMap_, iter->first))
+										{
+											//UDEBUG("Add new point %d to local map", iter->first);
+											pcl::PointXYZ newPt = util3d::transformPoint(iter->second, newPose);
+											wordsToAdd.insert(std::make_pair(iter->first, cv::Point3f(newPt.x, newPt.y, newPt.z)));
+										}
+									}
+
+									if((int)wordsToAdd.size())
+									{
+										localMap_.insert(wordsToAdd.begin(), wordsToAdd.end());
+										newPtsAdded = true;
+										UDEBUG("Added %d words", (int)wordsToAdd.size());
+									}
+
+									if(newPtsAdded)
+									{
+										keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
+										keyFramePoses_.insert(std::make_pair(newS->id(), newPose));
+
+										// keep only the two last signatures
+										while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
+										{
+											int nodeId = *memory_->getStMem().begin();
+											std::list<int> removedPts;
+											memory_->deleteLocation(nodeId, &removedPts);
+											keyFrameWords3D_.erase(nodeId);
+											keyFramePoses_.erase(nodeId);
+											for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
+											{
+												localMap_.erase(*iter);
+											}
+										}
+									}
+								}
+							}
+						}
+					}
+				}
+
+				if(!newPtsAdded)
+				{
+					// remove new words from dictionary
+					memory_->deleteLocation(newS->id());
+				}
+			}
+		}
+		else if(cornersMap_.size())
+		{
+			//flow
+
+			if(this->isInfoDataFilled() && info)
+			{
+				info->type = 1;
+			}
+
+			const Signature * refS = memory_->getLastWorkingSignature();
+
+			std::vector<cv::Point2f> refCorners(cornersMap_.size());
+			std::vector<cv::Point2f> refCornersGuess(cornersMap_.size());
+			std::vector<int> cornerIds(cornersMap_.size());
+			int ii=0;
+			for(std::map<int, cv::Point2f>::iterator iter=cornersMap_.begin(); iter!=cornersMap_.end(); ++iter)
+			{
+				std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
+				UASSERT(jter != refS->getWords().end());
+				refCorners[ii] = jter->second.pt;
+				refCornersGuess[ii] = iter->second;
+				cornerIds[ii] = iter->first;
+				++ii;
+			}
+
+			UDEBUG("flow");
+			// Find features in the new left image
+			std::vector<unsigned char> statusFlowInliers;
+			std::vector<float> err;
+			UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+			cv::calcOpticalFlowPyrLK(
+					refS->getImageRaw(),
+					newFrame,
+					refCorners,
+					refCornersGuess,
+					statusFlowInliers,
+					err,
+					cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+					cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+					cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
+			UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+			UDEBUG("Filtering optical flow outliers...");
+			float flow = 0;
+
+			if(this->isInfoDataFilled() && info)
+			{
+				info->refCorners = refCorners;
+				info->newCorners = refCornersGuess;
+			}
+
+			int oi = 0;
+			std::vector<cv::Point2f> tmpRefCorners(statusFlowInliers.size());
+			std::vector<cv::Point2f> newCorners(statusFlowInliers.size());
+			std::vector<int> inliersV(statusFlowInliers.size());
+			std::vector<int> tmpCornersId(statusFlowInliers.size());
+			UASSERT(refCornersGuess.size() == statusFlowInliers.size());
+			UASSERT(refCorners.size() == statusFlowInliers.size());
+			UASSERT(cornerIds.size() == statusFlowInliers.size());
+			for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
+			{
+				if(statusFlowInliers[i])
+				{
+					float dx = refCorners[i].x - refCornersGuess[i].x;
+					float dy = refCorners[i].y - refCornersGuess[i].y;
+					float tmp = std::sqrt(dx*dx + dy*dy);
+					flow+=tmp;
+
+					tmpRefCorners[oi] = refCorners[i];
+					newCorners[oi] = refCornersGuess[i];
+
+					inliersV[oi] = i;
+					cornersMap_.at(cornerIds[i]) = refCornersGuess[i];
+					tmpCornersId[oi] = cornerIds[i];
+
+					++oi;
+				}
+				else
+				{
+					cornersMap_.erase(cornerIds[i]);
+				}
+			}
+			if(oi)
+			{
+				flow /=float(oi);
+			}
+			tmpRefCorners.resize(oi);
+			newCorners.resize(oi);
+			inliersV.resize((oi));
+			tmpCornersId.resize(oi);
+			refCorners=  tmpRefCorners;
+			cornerIds = tmpCornersId;
+
+			if(this->isInfoDataFilled() && info)
+			{
+				// fill flow matches info
+				info->cornerInliers = inliersV;
+				inliers = (int)inliersV.size();
+			}
+
+			UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
+
+			if(flow > initMinFlow_ && oi > this->getMinInliers())
+			{
+				UDEBUG("flow=%f", flow);
+				// compute fundamental matrix
+				UDEBUG("Find fundamental matrix");
+				std::vector<unsigned char> statusFInliers;
+				cv::Mat F = cv::findFundamentalMat(
+						refCorners,
+						newCorners,
+						statusFInliers,
+						cv::RANSAC,
+						fundMatrixReprojError_,
+						fundMatrixConfidence_);
+				std::cout << "F=" << F << std::endl;
+
+				if(!F.empty())
+				{
+					UDEBUG("Filtering fundamental matrix outliers...");
+					std::vector<cv::Point2f> tmpNewCorners(statusFInliers.size());
+					std::vector<cv::Point2f> tmpRefCorners(statusFInliers.size());
+					tmpCornersId.resize(statusFInliers.size());
+					oi = 0;
+					UASSERT(newCorners.size() == statusFInliers.size());
+					UASSERT(refCorners.size() == statusFInliers.size());
+					UASSERT(cornerIds.size() == statusFInliers.size());
+					std::vector<int> tmpInliers(statusFInliers.size());
+					for(unsigned int i=0; i<statusFInliers.size(); ++i)
+					{
+						if(statusFInliers[i])
+						{
+							tmpNewCorners[oi] = newCorners[i];
+							tmpRefCorners[oi] = refCorners[i];
+							tmpInliers[oi] = inliersV[i];
+							tmpCornersId[oi] = cornerIds[i];
+							++oi;
+						}
+					}
+					tmpInliers.resize(oi);
+					tmpNewCorners.resize(oi);
+					tmpRefCorners.resize(oi);
+					tmpCornersId.resize(oi);
+					newCorners = tmpNewCorners;
+					refCorners = tmpRefCorners;
+					inliersV = tmpInliers;
+					cornerIds = tmpCornersId;
+					if(this->isInfoDataFilled() && info)
+					{
+						// update inliers
+						info->cornerInliers = inliersV;
+						inliers = (int)inliersV.size();
+					}
+					UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)statusFInliers.size());
+
+					if((int)refCorners.size() > this->getMinInliers())
+					{
+						std::vector<cv::Point2f> refCornersRefined;
+						std::vector<cv::Point2f> newCornersRefined;
+						//UDEBUG("Correcting matches...");
+						cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
+						UASSERT(refCorners.size() == refCornersRefined.size());
+						UASSERT(newCorners.size() == newCornersRefined.size());
+						refCorners = refCornersRefined;
+						newCorners = newCornersRefined;
+						//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 Kinv = K.inv();
+						cv::Mat E = K.t()*F*K;
+
+						//normalize coordinates
+						cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
+						cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
+						for(unsigned int i=0; i<refCorners.size(); ++i)
+						{
+							x.at<double>(0, i) = refCorners[i].x;
+							x.at<double>(1, i) = refCorners[i].y;
+							x.at<double>(2, i) = 1;
+
+							xp.at<double>(0, i) = newCorners[i].x;
+							xp.at<double>(1, i) = newCorners[i].y;
+							xp.at<double>(2, i) = 1;
+						}
+
+						cv::Mat x_norm = Kinv * x;
+						cv::Mat xp_norm = Kinv * xp;
+						x_norm = x_norm.rowRange(0,2);
+						xp_norm = xp_norm.rowRange(0,2);
+
+						cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
+						if(!P.empty())
+						{
+							cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
+							P0.at<double>(0,0) = 1;
+							P0.at<double>(1,1) = 1;
+							P0.at<double>(2,2) = 1;
+
+							UDEBUG("Computing P...done!");
+							std::cout << "P=" << P << std::endl;
+
+							cv::Mat R, T;
+							EpipolarGeometry::findRTFromP(P, R, T);
+
+							UDEBUG("");
+							std::vector<double> reprojErrors;
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+							EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
+
+							pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
+							std::vector<cv::Point2f> imagePoints(cloud->size());
+							inliersRef->resize(cloud->size());
+							inliersRefGuess->resize(cloud->size());
+							tmpCornersId.resize(cloud->size());
+
+							oi = 0;
+							UASSERT(newCorners.size() == cloud->size());
+							for(unsigned int i=0; i<cloud->size(); ++i)
+							{
+								if(cloud->at(i).z>0)
+								{
+									imagePoints[oi] = newCorners[i];
+									tmpCornersId[oi] = cornerIds[i];
+									(*inliersRef)[oi] = cloud->at(i);
+									if(!refDepth_.empty())
+									{
+										(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
+									}
+									++oi;
+								}
+							}
+							imagePoints.resize(oi);
+							inliersRef->resize(oi);
+							inliersRefGuess->resize(oi);
+							tmpCornersId.resize(oi);
+							cornerIds = tmpCornersId;
+
+							bool reject = false;
+
+							//estimate scale
+							float scale = 1;
+							std::multimap<float, float> scales; // <variance, scale>
+							if(!refDepth_.empty()) // scale known
+							{
+								UASSERT(inliersRefGuess->size() == inliersRef->size());
+								for(unsigned int i=0; i<inliersRef->size(); ++i)
+								{
+									if(pcl::isFinite(inliersRefGuess->at(i)))
+									{
+										float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
+										std::vector<float> errorSqrdDists(inliersRef->size());
+										for(unsigned int j=0; j<inliersRef->size(); ++j)
+										{
+											if(cloud->at(j).z>0)
+											{
+												pcl::PointXYZ refPt = inliersRef->at(j);
+												refPt.x *= s;
+												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);
+											}
+										}
+										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)
+								{
+									UWARN("Too high variance %f (should be < 0.01)");
+									reject = true; // 20 cm for good initialization
+								}
+
+							}
+							else if(inliersRef->size())
+							{
+								// find centroid of the cloud and set it to 1 meter
+								Eigen::Vector4f centroid;
+								pcl::compute3DCentroid(*inliersRef, centroid);
+								scale = 1.0f / centroid[2];
+								maxVariance_ = 0.01;
+							}
+							else
+							{
+								reject = true;
+							}
+
+							if(!reject)
+							{
+								//PnPRansac
+								std::vector<cv::Point3f> objectPoints(inliersRef->size());
+								for(unsigned int i=0; i<inliersRef->size(); ++i)
+								{
+									objectPoints[i].x = inliersRef->at(i).x * scale;
+									objectPoints[i].y = inliersRef->at(i).y * scale;
+									objectPoints[i].z = inliersRef->at(i).z * scale;
+								}
+								cv::Mat rvec;
+								cv::Mat tvec;
+								std::vector<int> inliersPnP;
+								cv::solvePnPRansac(
+										objectPoints, // 3D points in ref referential
+										imagePoints,  // 2D points in new referential
+										K,
+										cv::Mat(),
+										rvec,
+										tvec,
+										false,
+										this->getIterations(),
+										this->getPnPReprojError(),
+										0,
+										inliersPnP,
+										this->getPnPFlags());
+
+								UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
+
+								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));
+
+								output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
+								if(output.getNorm() < minTranslation_*5)
+								{
+									reject = true;
+									UWARN("Camera must be moved at least %f m for initialization (current=%f)",
+											minTranslation_*5, output.getNorm());
+								}
+
+								if(!reject)
+								{
+									///
+									std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
+									UASSERT(wordsId.size());
+									UASSERT(cornerIds.size() == objectPoints.size());
+									std::multimap<int, pcl::PointXYZ> keyFrameWords3D;
+									for(unsigned int i=0; i<inliersPnP.size(); ++i)
+									{
+										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());
+										localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
+										keyFrameWords3D.insert(std::make_pair(id, pt));
+									}
+
+									keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), keyFrameWords3D));
+									keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
+								}
+							}
+						}
+						else
+						{
+							UERROR("No valid camera matrix found!");
+						}
+					}
+					else
+					{
+						UWARN("Not enough inliers %d/%d", (int)refCorners.size(), this->getMinInliers());
+					}
+				}
+				else
+				{
+					UWARN("Fundamental matrix not found!");
+				}
+			}
+			else
+			{
+				UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
+			}
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+
+			// generate kpts
+		if(memory_->update(SensorData(newFrame)))
+		{
+			const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
+			if((int)words.size() > this->getMinInliers())
+			{
+				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));
+				}
+				refDepth_ = data.depth().clone();
+				keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
+			}
+			else
+			{
+				UWARN("Too low 2D corners (%d), ignoring new frame...",
+						(int)words.size());
+				memory_->deleteLocation(memory_->getLastSignatureId());
+			}
+		}
+		else
+		{
+			UERROR("Failed creating signature");
+		}
+	}
+
+	memory_->emptyTrash();
+
+	if(this->isInfoDataFilled() && info)
+	{
+		//info->variance = variance;
+		info->inliers = inliers;
+		info->matches = correspondences;
+		info->features = nFeatures;
+		info->localMapSize = (int)localMap_.size();
+		info->localMap = localMap_;
+	}
+
+	UINFO("Odom update=%fs tf=[%s] inliers=%d/%d, local_map[%d]=%d, accepted=%s",
+			timer.elapsed(),
+			output.prettyPrint().c_str(),
+			inliers,
+			correspondences,
+			(int)memory_->getStMem().size(),
+			(int)localMap_.size(),
+			!output.isNull()?"true":"false");
+
+	return output;
+}
+
+} // namespace rtabmap

corelib/src/OdometryOpticalFlow.cpp

@@ -0,0 +1,951 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/video/tracking.hpp>
+
+namespace rtabmap {
+
+OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
+	Odometry(parameters),
+	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+	flowIterations_(Parameters::defaultOdomFlowIterations()),
+	flowEps_(Parameters::defaultOdomFlowEps()),
+	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	stereoWinSize_(Parameters::defaultStereoWinSize()),
+	stereoIterations_(Parameters::defaultStereoIterations()),
+	stereoEps_(Parameters::defaultStereoEps()),
+	stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
+	stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
+	subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
+	subPixIterations_(Parameters::defaultOdomSubPixIterations()),
+	subPixEps_(Parameters::defaultOdomSubPixEps()),
+	refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+{
+	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
+	Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
+
+	ParametersMap::const_iterator iter;
+	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
+	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+	{
+		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
+	}
+
+	ParametersMap customParameters;
+	int maxFeatures = Parameters::defaultOdomMaxFeatures();
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	feature2D_ = Feature2D::create(detectorStrategy, customParameters);
+}
+
+OdometryOpticalFlow::~OdometryOpticalFlow()
+{
+	delete feature2D_;
+}
+
+
+void OdometryOpticalFlow::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+	refFrame_ = cv::Mat();
+	refCorners_.clear();
+	refCorners3D_->clear();
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpticalFlow::computeTransform(
+		const SensorData & data,
+		OdometryInfo * info)
+{
+	UDEBUG("");
+
+	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)
+	{
+		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newLeftFrame = data.image().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("");
+		// Find features in the new left image
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		cv::calcOpticalFlowPyrLK(
+				refFrame_,
+				newLeftFrame,
+				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");
+
+		std::vector<cv::Point2f> lastCornersKept(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];
+				newCornersKept[ki] = newCorners[i];
+				++ki;
+			}
+		}
+		lastCornersKept.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())
+			{
+				// find correspondences
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(statusLast.size());
+					info->newCorners.resize(statusLast.size());
+				}
+
+				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;
+
+				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,
+							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
+			{
+				UDEBUG("Getting correspondences begin");
+				// Get 3D correspondences
+				pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(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;
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(statusLast.size());
+					info->newCorners.resize(statusLast.size());
+				}
+				for(unsigned int i=0; i<statusLast.size(); ++i)
+				{
+					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());
+
+							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())))
+							{
+								//Add 3D correspondences!
+								lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
+								newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
+								correspondencesLast->at(oi) = lastPt3D;
+								correspondencesNew->at(oi) = newPt3D;
+								if(this->isInfoDataFilled() && info)
+								{
+									info->refCorners[oi] = lastCornersKept[i];
+									info->newCorners[oi] = newCornersKept[i];
+								}
+								++oi;
+							}
+						}
+					}
+				}// end loop
+				correspondencesLast->resize(oi);
+				correspondencesNew->resize(oi);
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(oi);
+					info->newCorners.resize(oi);
+				}
+				correspondences = oi;
+				refCorners3D_ = correspondencesNew;
+				UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
+
+				if(correspondences >= this->getMinInliers())
+				{
+					std::vector<int> inliersV;
+					UTimer timerRANSAC;
+					Transform t = 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(!t.isNull() && inliers >= this->getMinInliers())
+					{
+						output = t;
+					}
+					else
+					{
+						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+					}
+
+					if(this->isInfoDataFilled() && info)
+					{
+						info->cornerInliers = inliersV;
+					}
+				}
+				else
+				{
+					UWARN("Not enough correspondences (%d)", correspondences);
+				}
+			}
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+	}
+
+	newCorners.clear();
+	if(!output.isNull())
+	{
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			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(newLeftFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					UDEBUG("cv::cornerSubPix() begin");
+					cv::cornerSubPix(newLeftFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+					UDEBUG("cv::cornerSubPix() end");
+				}
+			}
+		}
+
+		if((int)newCorners.size() > this->getMinInliers())
+		{
+			refFrame_ = newLeftFrame;
+			refRightFrame_ = newRightFrame;
+			refCorners_ = newCorners;
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+			output.setNull();
+		}
+	}
+
+	if(info)
+	{
+		info->type = 1;
+		info->variance = variance;
+		info->inliers = inliers;
+		info->features = (int)newCorners.size();
+		info->matches = correspondences;
+	}
+
+	UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			output.isNull()?"true":"false",
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			!output.isNull()?"true":"false");
+
+	return output;
+}
+
+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

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

corelib/src/Parameters.cpp

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

corelib/src/RtabmapThread.cpp

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/UserDataEvent.h"
+#include "rtabmap/core/Memory.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
@@ -48,7 +50,8 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_rtabmap(rtabmap),
 		_paused(false),
 		lastPose_(Transform::getIdentity()),
-		_variance(0)
+		_rotVariance(0),
+		_transVariance(0)
 
 {
 	UASSERT(rtabmap != 0);
@@ -85,9 +88,16 @@ void RtabmapThread::clearBufferedData()
 	{
 		_dataBuffer.clear();
 		lastPose_.setIdentity();
-		_variance = 0;
+		_rotVariance = 0;
+		_transVariance = 0;
 	}
 	_dataMutex.unlock();
+
+	_userDataMutex.lock();
+	{
+		_userData = cv::Mat();
+	}
+	_userDataMutex.unlock();
 }
 
 void RtabmapThread::setDetectorRate(float rate)
@@ -108,18 +118,27 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
+	std::map<int, double> stamps;
+	std::map<int, std::string> labels;
+	std::map<int, std::vector<unsigned char> > userDatas;
 
 	_rtabmap->get3DMap(signatures,
 			poses,
 			constraints,
 			mapIds,
+			stamps,
+			labels,
+			userDatas,
 			optimized,
 			full);
 
 	this->post(new RtabmapEvent3DMap(signatures,
 			poses,
 			constraints,
-			mapIds));
+			mapIds,
+			stamps,
+			labels,
+			userDatas));
 }
 
 void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
@@ -128,17 +147,26 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 	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));
+			mapIds,
+			stamps,
+			labels,
+			userDatas));
 }
 
 
@@ -167,6 +195,8 @@ void RtabmapThread::mainLoop()
 	}
 	_stateMutex.unlock();
 
+	int id = 0;
+	std::vector<unsigned char> userData;
 	switch(state)
 	{
 	case kStateDetecting:
@@ -235,6 +265,27 @@ void RtabmapThread::mainLoop()
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();
 		break;
+	case kStateAddingUserData:
+		_userDataMutex.lock();
+		{
+			userData = _userData;
+			_userData.clear();
+		}
+		_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 = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("goal_label"));
+		}
+		if(id <= 0 || !_rtabmap->computePath(id, true))
+		{
+			UERROR("Failed to set a goal to location=%d.", id);
+		}
+		this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
+		break;
 	default:
 		UFATAL("Invalid state !?!?");
 		break;
@@ -266,6 +317,32 @@ void RtabmapThread::handleEvent(UEvent* event)
 			lastPose_.setNull();
 		}
 	}
+	else if(event->getClassName().compare("UserDataEvent") == 0)
+	{
+		if(!_paused)
+		{
+			UDEBUG("UserDataEvent");
+			bool updated = false;
+			UserDataEvent * e = (UserDataEvent*)event;
+			_userDataMutex.lock();
+			if(!e->data().empty())
+			{
+				updated = !_userData.empty();
+				_userData = e->data();
+			}
+			_userDataMutex.unlock();
+			if(updated)
+			{
+				UWARN("New user data received before the last one was processed... replacing "
+					"user data with this new one. Note that UserDataEvent should be used only "
+					"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
+			}
+			else
+			{
+				pushNewState(kStateAddingUserData);
+			}
+		}
+	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 	{
 		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
@@ -385,6 +462,14 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_PAUSE");
 			_paused = !_paused;
 		}
+		else if(cmd == RtabmapEventCmd::kCmdGoal)
+		{
+			ULOGGER_DEBUG("CMD_GOAL");
+			ParametersMap param;
+			param.insert(ParametersPair("goal_label", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateSettingGoal, param);
+		}
 		else
 		{
 			UWARN("Cmd %d unknown!", cmd);
@@ -437,13 +522,18 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
-			_variance = 0;
+			_rotVariance = 0;
+			_transVariance = 0;
 		}
 
 		lastPose_ = sensorData.pose();
-		if(sensorData.poseVariance() > _variance)
+		if(sensorData.poseRotVariance() > _rotVariance)
 		{
-			_variance = sensorData.poseVariance();
+			_rotVariance = sensorData.poseRotVariance();
+		}
+		if(sensorData.poseTransVariance() > _transVariance)
+		{
+			_transVariance = sensorData.poseTransVariance();
 		}
 
 		if(_rate>0.0f)
@@ -459,12 +549,17 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		_dataMutex.lock();
 		{
 			_dataBuffer.push_back(sensorData);
-			if(_variance <= 0)
+			if(_rotVariance <= 0)
 			{
-				_variance = 1.0f;
+				_rotVariance = 1.0f;
 			}
-			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _variance);
+			if(_transVariance <= 0)
-			_variance = 0;
+			{
+				_transVariance = 1.0f;
+			}
+			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
+			_rotVariance = 0;
+			_transVariance = 0;
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
 			{
 				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");

corelib/src/SensorData.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/SensorData.h"
 #include "rtabmap/utilite/ULogger.h"
+#include <rtabmap/utilite/UMath.h>
 
 namespace rtabmap
 {
@@ -36,29 +37,38 @@ namespace rtabmap
  * An id is automatically generated if id=0.
  */
 SensorData::SensorData() :
-	_image(cv::Mat()),
 	_id(0),
+	_stamp(0.0),
 	_fx(0.0f),
 	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity()),
-	_poseVariance(1.0f)
+	_poseRotVariance(1.0f),
+	_poseTransVariance(1.0f),
+	_laserScanMaxPts(0)
 {
 }
 
 SensorData::SensorData(const cv::Mat & image,
-	  int id) :
+	  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()),
-	_poseVariance(1.0f)
+	_poseRotVariance(1.0f),
+	_poseTransVariance(1.0f),
+	_laserScanMaxPts(0),
+	_userData(userData)
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
 }
 
@@ -71,10 +81,14 @@ SensorData::SensorData(const cv::Mat & image,
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id) :
+		  float poseTransVariance,
+		  int id,
+		  double stamp,
+		  const std::vector<unsigned char> & userData) :
 	_image(image),
 	_id(id),
+	_stamp(stamp),
 	_depthOrRightImage(depthOrRightImage),
 	_fx(fx),
 	_fyOrBaseline(fyOrBaseline),
@@ -82,19 +96,25 @@ SensorData::SensorData(const cv::Mat & image,
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_poseVariance(poseVariance)
+	_poseRotVariance(poseRotVariance),
+	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(0),
+	_userData(userData)
 {
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+	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(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
 	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)");
 }
 
 	// Metric constructor + 2d depth
 SensorData::SensorData(const cv::Mat & laserScan,
+		  int laserScanMaxPts,
 		  const cv::Mat & image,
 		  const cv::Mat & depthOrRightImage,
 		  float fx,
@@ -103,10 +123,14 @@ SensorData::SensorData(const cv::Mat & laserScan,
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id) :
+		  float poseTransVariance,
+		  int id,
+		  double stamp,
+		  const std::vector<unsigned char> & userData) :
 	_image(image),
 	_id(id),
+	_stamp(stamp),
 	_depthOrRightImage(depthOrRightImage),
 	_laserScan(laserScan),
 	_fx(fx),
@@ -115,16 +139,21 @@ SensorData::SensorData(const cv::Mat & laserScan,
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_poseVariance(poseVariance)
+	_poseRotVariance(poseRotVariance),
+	_poseTransVariance(poseTransVariance),
+	_laserScanMaxPts(laserScanMaxPts),
+	_userData(userData)
 {
 	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
-	UASSERT(image.type() == CV_8UC1 || // Mono
+	UASSERT(image.empty() ||
+			image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
-	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+	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(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
 	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)");
 }
 
 bool SensorData::empty() const

corelib/src/Signature.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/Memory.h"
-#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Compression.h"
 #include <opencv2/highgui/highgui.hpp>
 
 #include <rtabmap/utilite/UtiLite.h>
@@ -39,6 +39,7 @@ namespace rtabmap
 Signature::Signature() :
 	_id(0), // invalid id
 	_mapId(-1),
+	_stamp(0.0),
 	_weight(-1),
 	_saved(false),
 	_modified(true),
@@ -47,16 +48,21 @@ Signature::Signature() :
 	_fx(0.0f),
 	_fy(0.0f),
 	_cx(0.0f),
-	_cy(0.0f)
+	_cy(0.0f),
+	_laserScanMaxPts(0)
 {
 }
 
 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, // 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
@@ -64,10 +70,14 @@ Signature::Signature(
 		float fy,
 		float cx,
 		float cy,
-		const Transform & localTransform) :
+		const Transform & localTransform,
+		int laserScanMaxPts) :
 	_id(id),
 	_mapId(mapId),
-	_weight(0),
+	_stamp(stamp),
+	_weight(weight),
+	_label(label),
+	_userData(userData),
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
@@ -82,7 +92,8 @@ Signature::Signature(
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_words3(words3)
+	_words3(words3),
+	_laserScanMaxPts(laserScanMaxPts)
 {
 }
 
@@ -91,6 +102,18 @@ 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)
@@ -149,6 +172,21 @@ void Signature::removeLink(int idTo)
 	}
 }
 
+void Signature::removeVirtualLinks()
+{
+	for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
+	{
+		if(iter->second.type() == Link::kVirtualClosure)
+		{
+			_links.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+}
+
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
@@ -211,26 +249,42 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
 	_cy=cy;
 }
 
-SensorData Signature::toSensorData()
+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
 {
-	this->uncompressData();
+	rotVariance = 1.0f;
-	float variance = 1.0f;
+	transVariance = 1.0f;
 	if(_links.size())
 	{
-		for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
+		for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
 		{
 			if(iter->second.kNeighbor)
 			{
 				//Assume the first neighbor to be the backward neighbor link
 				if(iter->second.to() < iter->second.from())
 				{
-					variance = iter->second.variance();
+					rotVariance = iter->second.rotVariance();
+					transVariance = iter->second.transVariance();
 					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,
@@ -239,8 +293,11 @@ SensorData Signature::toSensorData()
 			_cy,
 			_localTransform,
 			_pose,
-			variance,
+			rotVariance,
-			_id);
+			transVariance,
+			_id,
+			_stamp,
+			_userData);
 }
 
 void Signature::uncompressData()
@@ -283,9 +340,9 @@ void Signature::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()))
 	{
-		util3d::CompressionThread ctImage(_imageCompressed, true);
+		rtabmap::CompressionThread ctImage(_imageCompressed, true);
-		util3d::CompressionThread ctDepth(_depthCompressed, true);
+		rtabmap::CompressionThread ctDepth(_depthCompressed, true);
-		util3d::CompressionThread ctLaserScan(_laserScanCompressed, false);
+		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
 		if(imageRaw && imageRaw->empty())
 		{
 			ctImage.start();

corelib/src/Statistics.cpp

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

corelib/src/Transform.cpp

@@ -74,7 +74,7 @@ Transform::Transform(float r11, float r12, float r13, float o14,
 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
 {
 	Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
-	*this = util3d::transformFromEigen3f(t);
+	*this = fromEigen3f(t);
 }
 
 bool Transform::isNull() const
@@ -131,10 +131,16 @@ void Transform::setIdentity()
 	*this = getIdentity();
 }
 
+float Transform::theta() const
+{
+	float roll, pitch, yaw;
+	this->getEulerAngles(roll, pitch, yaw);
+	return yaw;
+}
+
 Transform Transform::inverse() const
 {
-	Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
+	return fromEigen4f(toEigen4f().inverse());
-	return util3d::transformFromEigen4f(m.inverse());
 }
 
 Transform Transform::rotation() const
@@ -153,7 +159,13 @@ Transform Transform::translation() const
 
 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
 {
-	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
+	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
+}
+
+void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
+{
+	float x,y,z;
+	pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
 }
 
 void Transform::getTranslation(float & x, float & y, float & z) const
@@ -165,12 +177,22 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 
 float Transform::getNorm() const
 {
-	return std::sqrt(this->getNormSquared());
+	return uNorm(this->x(), this->y(), this->z());
 }
 
 float Transform::getNormSquared() const
 {
-	return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
+	return uNormSquared(this->x(), this->y(), this->z());
+}
+
+float Transform::getDistance(const Transform & t) const
+{
+	return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
+}
+
+float Transform::getDistanceSquared(const Transform & t) const
+{
+	return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
 }
 
 std::string Transform::prettyPrint() const
@@ -182,9 +204,7 @@ std::string Transform::prettyPrint() const
 
 Transform Transform::operator*(const Transform & t) const
 {
-	Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
+	return fromEigen4f(toEigen4f()*t.toEigen4f());
-	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
-	return util3d::transformFromEigen4f(m1*m2);
 }
 
 Transform & Transform::operator*=(const Transform & t)
@@ -216,5 +236,87 @@ std::ostream& operator<<(std::ostream& os, const Transform& s)
 	return os;
 }
 
+Eigen::Matrix4f Transform::toEigen4f() const
+{
+	Eigen::Matrix4f m;
+	m << data_[0], data_[1], data_[2], data_[3],
+		 data_[4], data_[5], data_[6], data_[7],
+		 data_[8], data_[9], data_[10], data_[11],
+		 0,0,0,1;
+	return m;
+}
+Eigen::Matrix4d Transform::toEigen4d() const
+{
+	Eigen::Matrix4d m;
+	m << data_[0], data_[1], data_[2], data_[3],
+		 data_[4], data_[5], data_[6], data_[7],
+		 data_[8], data_[9], data_[10], data_[11],
+		 0,0,0,1;
+	return m;
+}
+
+Eigen::Affine3f Transform::toEigen3f() const
+{
+	return Eigen::Affine3f(toEigen4f());
+}
+
+Eigen::Affine3d Transform::toEigen3d() const
+{
+	return Eigen::Affine3d(toEigen4d());
+}
+
+Eigen::Quaternionf Transform::getQuaternionf() const
+{
+	return Eigen::Quaternionf(this->toEigen3f().rotation()).normalized();
+}
+
+Eigen::Quaterniond Transform::getQuaterniond() const
+{
+	return Eigen::Quaterniond(this->toEigen3d().rotation()).normalized();
+}
+
+Transform Transform::getIdentity()
+{
+	return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
+}
+
+Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+Transform Transform::fromEigen3f(const Eigen::Affine3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+Transform Transform::fromEigen3f(const Eigen::Isometry3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
 
 }

corelib/src/VWDictionary.cpp

@@ -71,7 +71,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 
-	UASSERT(_nndrRatio > 0.0f);
+	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
 	std::string dictionaryPath = _dictionaryPath;
 	bool incrementalDictionary = _incrementalDictionary;
@@ -167,7 +167,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 							//get descriptor
 							for(;i<dimension && iter != strList.end(); ++i, ++iter)
 							{
-								descriptor.at<float>(i) = std::atof(iter->c_str());
+								descriptor.at<float>(i) = uStr2Float(*iter);
 							}
 							if(i != dimension)
 							{
@@ -209,6 +209,10 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	{
 		_incrementalDictionary = false;
 	}
+	else if(_incrementalDictionary)
+	{
+		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
+	}
 	_dictionaryPath = dictionaryPath;
 }
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -17,27 +17,32 @@ CREATE TABLE Node (
 	id INTEGER NOT NULL,
 	map_id INTEGER NOT NULL,
 	weight INTEGER,
+	stamp FLOAT,
 	pose BLOB,
+	label TEXT,
+	user_data BLOB,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
 
 CREATE TABLE Image (
 	id INTEGER NOT NULL,
-	data BLOB,
+	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,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
+	data BLOB,       -- compressed image (Depth or Right image)
 	fx FLOAT,
-	fy FLOAT,
+	fy FLOAT,        -- baseline if stereo
 	cx FLOAT,
 	cy FLOAT,
 	local_transform BLOB,
-	data2d BLOB,   -- CV_32FC2, Example: Laser scan
+	data2d BLOB,     -- compressed data (Laser scan)
+	data2d_max_pts INTEGER,   -- Laser scan max points
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
@@ -46,7 +51,8 @@ CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
 	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
-	variance FLOAT NOT NULL,
+	rot_variance FLOAT NOT NULL,
+	trans_variance FLOAT NOT NULL,
 	transform BLOB,
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)
@@ -121,6 +127,7 @@ END;
 -- *******************************************************************
 CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
 CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
 
 -- *******************************************************************
 -- VERSION

corelib/src/toro3d/posegraph2.cpp

@@ -0,0 +1,442 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ *   TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                           Slawomir Grzonka and  Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph2.cpp
+ * 
+ * \brief Defines the graph of 2D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#include "posegraph2.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+
+typedef unsigned int uint;
+#define LINESIZE 81920
+
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+
+bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
+  clear();
+  ifstream is(filename);
+  if (!is)
+    return false;
+
+  while(is){
+    char buf[LINESIZE];
+    is.getline(buf,LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+
+    if (tag=="VERTEX" || tag=="VERTEX2"){
+      int id;
+      Pose p;
+      ls >> id >> p.x() >> p.y() >> p.theta(); 
+      if (addVertex(id,p))
+ 	DEBUG(2) << "V " << id << endl;
+      
+    }
+
+    if (tag=="EDGE" || tag=="EDGE2"){
+      int id1, id2;
+      Pose p;
+      InformationMatrix m;
+      ls >> id1 >> id2 >> p.x() >> p.y() >> p.theta();
+      if (overrideCovariances){
+	m.values[0][0]=1;  m.values[1][1]=1; m.values[2][2]=1;
+	m.values[0][1]=0;  m.values[0][2]=0; m.values[1][2]=0;
+      } else {
+	ls >> m.values[0][0] >> m.values[0][1] >> m.values [1][1]
+	   >> m.values[2][2] >> m.values[0][2] >> m.values [1][2];
+      }
+      m.values[1][0]=m.values[0][1];
+      m.values[2][0]=m.values[0][2];
+      m.values[2][1]=m.values[1][2];
+      TreePoseGraph2::Vertex* v1=vertex(id1);
+      TreePoseGraph2::Vertex* v2=vertex(id2);
+      Transformation t(p);
+      if (addEdge(v1, v2,t ,m))
+	 DEBUG(2) << "E " << id1 << " " << id2 <<  endl;
+    }
+  }
+  return true;
+}
+
+bool TreePoseGraph2::loadEquivalences(const char* filename){
+  ifstream is(filename);
+  if (!is)
+    return false;
+  EdgeList suppressed;
+  uint equivCount=0;
+  while (is){
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    if (tag=="EQUIV"){
+      int id1, id2;
+      ls >> id1 >> id2;
+      Edge* e=edge(id1,id2);
+      if (!e)
+	e=edge(id2,id1);
+      if (e){
+	suppressed.push_back(e);
+	equivCount++;
+      }
+    }
+  }
+  for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
+    Edge* e=*it;
+    if (e->v1->id > e->v2->id)
+      revertEdge(e);
+    collapseEdge(e);
+  }
+  for (TreePoseGraph2::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->edges.clear();
+  }
+  for (TreePoseGraph2::EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    TreePoseGraph2::Edge * e=it->second;
+    e->v1->edges.push_back(e);
+    e->v2->edges.push_back(e);
+  }
+  return true;
+}
+
+bool TreePoseGraph2::saveGnuplot(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    const Vertex* v1=e->v1;
+    const Vertex* v2=e->v2;
+    
+    os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.theta() << endl;
+    os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.theta() << endl;
+    os << endl;
+  }
+  return true;
+
+}
+
+bool TreePoseGraph2::save(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  
+  for (TreePoseGraph2::VertexMap::const_iterator it=vertices.begin(); it!=vertices.end(); it++){
+    const TreePoseGraph2::Vertex* v=it->second;
+    os << "VERTEX " 
+       << v->id << " " 
+       << v->pose.x() << " " 
+       << v->pose.y() << " " 
+       << v->pose.theta()<< endl; 
+  }
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.theta() << " ";
+    os << e->informationMatrix.values[0][0] << " "
+       << e->informationMatrix.values[0][1] << " "
+       << e->informationMatrix.values[1][1] << " "
+       << e->informationMatrix.values[2][2] << " "
+       << e->informationMatrix.values[0][2] << " "
+       << e->informationMatrix.values[1][2] << endl;
+    }
+  return true;
+}
+
+/** \brief A class (struct) used to print vertex information to a
+    stream. Needed for debugging. **/
+struct IdPrinter{
+  IdPrinter(std::ostream& _os):os(_os){}
+  std::ostream& os;
+  void perform(TreePoseGraph2::Vertex* v){
+    std::cout << "(" << v->id << "," << v->level << ")" << endl;
+  }
+};
+
+void TreePoseGraph2::printDepth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeDepthVisit(ip, root);
+}
+
+void TreePoseGraph2::printWidth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeBreadthVisit(ip);
+}
+
+/** \brief A class (struct) for realizing the pose update of the
+    individual nodes. Assumes the correct order of constraint updates
+    (according to the tree level, see RSS07 paper)**/
+struct PosePropagator{
+  void perform(TreePoseGraph2::Vertex* v){
+    if (!v->parent)
+      return;
+    TreePoseGraph2::Transformation tParent(v->parent->pose);
+    TreePoseGraph2::Transformation tNode=tParent*v->parentEdge->transformation;
+
+    //cerr << "EDGE(" << v->parentEdge->v1->id << "," << v->parentEdge->v2->id <<"): " << endl;
+    //Pose pParent=v->parent->pose;
+    //cerr << "     p=" << pParent.x() << "," << pParent.y() << "," << pParent.theta() <<  endl;
+    //Pose pEdge=v->parentEdge->transformation.toPoseType();
+    //cerr << "     m=" << pEdge.x() << "," << pEdge.y() << "," << pEdge.theta() <<  endl;
+    //Pose pNode=tNode.toPoseType();
+    //cerr << "     n=" << pNode.x() << "," << pNode.y() << "," << pNode.theta() <<  endl;
+
+    assert(v->parentEdge->v1==v->parent);
+    assert(v->parentEdge->v2==v);
+    v->pose=tNode.toPoseType();
+  }
+};
+
+void TreePoseGraph2::initializeOnTree(){
+  PosePropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+
+void TreePoseGraph2::printEdgesStat(std::ostream& os){
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.theta() << " ";
+    os << e->informationMatrix.values[0][0] << " "
+       << e->informationMatrix.values[0][1] << " "
+       << e->informationMatrix.values[1][1] << " "
+       << e->informationMatrix.values[2][2] << " "
+       << e->informationMatrix.values[0][2] << " "
+       << e->informationMatrix.values[1][2] << endl;
+    os << "   top=" << e->top->id << " length=" << e->length << endl;
+  }
+}
+
+void TreePoseGraph2::revertEdgeInfo(Edge* e){
+  Transformation it=e->transformation.inv();
+  InformationMatrix R;
+  R.values[0][0]=e->transformation.rotationMatrix[0][0];
+  R.values[0][1]=e->transformation.rotationMatrix[0][1];
+  R.values[0][2]=0;
+
+  R.values[1][0]=e->transformation.rotationMatrix[1][0];
+  R.values[1][1]=e->transformation.rotationMatrix[1][1];
+  R.values[1][2]=0;
+
+  R.values[2][0]=0;
+  R.values[2][1]=0;
+  R.values[2][2]=1;
+
+  InformationMatrix IM=R.transpose()*e->informationMatrix*R;
+
+
+  //Pose np=e->transformation.toPoseType();
+ 
+  //Pose ip=it.toPoseType();
+
+  //Transformation tc=it*e->transformation;
+  //Pose pc=tc.toPoseType();
+
+  e->transformation=it;
+  e->informationMatrix=IM;
+};
+
+void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
+  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
+  v->transformation=tp;
+  v->pose=tp.toPoseType();
+  v->parameters=v->pose;
+  v->parameters.x()-=v->parent->pose.x();
+  v->parameters.y()-=v->parent->pose.y();
+  v->parameters.theta()-=v->parent->pose.theta();
+  v->parameters.theta()=atan2(sin(v->parameters.theta()), cos(v->parameters.theta()));
+}
+
+void TreePoseGraph2::collapseEdge(Edge* e){
+  EdgeMap::iterator ie_it=edges.find(e);
+  if (ie_it==edges.end())
+    return;
+  //VertexMap::iterator it1=vertices.find(e->v1->id);
+  //VertexMap::iterator it2=vertices.find(e->v2->id);
+  assert(vertices.find(e->v1->id)!=vertices.end());
+  assert(vertices.find(e->v2->id)!=vertices.end());
+
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+  
+  
+  // all the edges of v2 become outgoing
+  for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
+    if ( (*it)->v1!=v2 )
+      revertEdge(*it);
+  }
+
+ // all the edges of v1 become outgoing
+  for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
+    if ( (*it)->v1!=v1 )
+      revertEdge(*it);
+  }
+
+  assert(e->v1==v1);
+
+  InformationMatrix I12=e->informationMatrix;
+  CovarianceMatrix  C12=I12.inv();
+  Transformation    T12=e->transformation;
+  //Pose              p12=T12.toPoseType();
+
+  //Transformation iT12=T12.inv();
+
+  //compute the marginal information of the nodes in the path v1-v2-v*
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e2=*it2;
+    if (e2->v1==v2){ //edge leaving v2
+      //Transformation T2x=e2->transformation;
+      //Pose           p2x=T2x.toPoseType();
+      InformationMatrix I2x=e2->informationMatrix;
+      CovarianceMatrix  C2x=I2x.inv();
+      
+      //compute the estimate of the vertex based on the path v1-v2-vx
+      
+      //Transformation tr=iT12*T2x;
+      
+      //InformationMatrix R;
+      //R.values[0][0]=tr.rotationMatrix[0][0];
+      //R.values[0][1]=tr.rotationMatrix[0][1];
+      //R.values[0][2]=0;
+      
+      //R.values[1][0]=tr.rotationMatrix[1][0];
+      //R.values[1][1]=tr.rotationMatrix[1][1];
+      //R.values[1][2]=0;
+      
+      //R.values[2][0]=0;
+      //R.values[2][1]=0;
+      //R.values[2][2]=1;
+
+      //CovarianceMatrix CM=R.transpose()*C2x*R;
+      
+      
+      Transformation T1x_pred=T12*e2->transformation;
+      Covariance C1x_pred=C12+C2x;
+      InformationMatrix I1x_pred=C1x_pred.inv();
+
+      e2->transformation=T1x_pred;
+      e2->informationMatrix=I1x_pred;
+    }
+  }
+
+  //all the edges leaving v1 and leaving v2 and leading to the same point are merged
+  std::list<Transformation> tList;
+  std::list<InformationMatrix> iList;
+  std::list<Vertex*> vList;
+
+  //others are transformed and added to v1
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e1x=0;
+    Edge* e2x=0;
+    if ( ((*it2)->v1!=v1)){
+      e2x=*it2;
+      for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
+	if ((*it1)->v2==(*it2)->v2)
+	  e1x=*it1;
+      }
+
+    }
+    if (e1x && e2x){
+      Transformation t1x=e1x->transformation;
+      InformationMatrix I1x=e1x->informationMatrix;
+      Pose p1x=t1x.toPoseType();
+
+      Transformation t2x=e2x->transformation;
+      InformationMatrix I2x=e2x->informationMatrix;;
+      Pose p2x=t2x.toPoseType();
+
+      InformationMatrix IM=I1x+I2x;
+      CovarianceMatrix  CM=IM.inv();
+      InformationMatrix scale1=CM*I1x;
+      InformationMatrix scale2=CM*I2x;
+
+
+      Pose p1=scale1*p1x;
+      Pose p2=scale2*p2x;
+
+
+      //need to recover the angles in a decent way.
+      double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
+      double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
+      
+      DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
+      DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
+      
+      Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
+      DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
+      
+      e1x->transformation=Transformation(pFinal);
+      e1x->informationMatrix=IM;
+    }
+    if (!e1x && e2x){
+      tList.push_back(e2x->transformation);
+      iList.push_back(e2x->informationMatrix);
+      vList.push_back(e2x->v2);
+    }
+  }
+  removeVertex(v2->id);
+
+  std::list<Transformation>::iterator t=tList.begin();
+  std::list<InformationMatrix>::iterator i=iList.begin();
+  std::list<Vertex*>::iterator v=vList.begin();
+  while (i!=iList.end()){
+    addEdge(v1,*v,*t,*i);
+    i++;
+    t++;
+    v++;
+  }
+}
+
+}; //namespace AISNavigation

corelib/src/toro3d/posegraph2.hh

@@ -0,0 +1,110 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka and  Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph2.hh
+ *
+ * \brief Defines the graph of 2D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#ifndef _POSEGRAPH2_HH_
+#define _POSEGRAPH2_HH_
+
+#include "posegraph.hh"
+#include "transformation2.hh"
+#include <iostream>
+#include <vector>
+
+namespace AISNavigation {
+
+
+
+/** \brief The class (struct) that contains 2D graph related functions
+    such as loading, saving, merging, etc. **/
+struct TreePoseGraph2: public TreePoseGraph< Operations2D<double> >{
+
+  typedef Operations2D<double>::PoseType           Pose;
+  typedef Operations2D<double>::RotationType       Rotation;
+  typedef Operations2D<double>::TranslationType    Translation;
+  typedef Operations2D<double>::TransformationType Transformation;
+  typedef Operations2D<double>::CovarianceType     CovarianceMatrix;
+  typedef Operations2D<double>::InformationType    InformationMatrix;
+  
+  /** Load a graph from a file ignoring the equivalence constraints
+      @param filename the graph file
+      @param overrideCovariances ignore the covariances from the file, and use identities instead
+   **/
+  bool load( const char* filename, bool overrideCovariances=false); 
+
+  /** Load only the equivalence constraints from a  graph file (call load before)  **/
+  bool loadEquivalences( const char* filename); 
+
+  /** Saves the graph in the graph-format**/
+  bool save( const char* filename);
+
+  /** Saved the graph for visualizing it using gnuplot **/
+  bool saveGnuplot( const char* filename);
+
+  /** Debug function  **/
+  void printDepth( std::ostream& os );
+
+  /** Debug function  **/
+  void printWidth( std::ostream& os );
+
+  /** Debug function  **/
+  void printEdgesStat( std::ostream& os);
+
+  void initializeOnTree();
+  
+  /** Turn around the edge (<i,j>  => <j,i>)  **/
+  virtual void revertEdgeInfo(Edge* e);
+
+  virtual void initializeFromParentEdge(Vertex* v);
+
+  /** Function to compress a graph. Needed if, for example, equivalence
+      constraints are used to build a graoh structure with indices
+      without gaps.  **/
+  virtual void collapseEdge(Edge* e);
+
+  /** Specifies the verbose level for debugging **/  
+  int verboseLevel;
+};
+
+}; //namespace AISNavigation
+#endif
+
+
+

corelib/src/toro3d/posegraph3.cpp

@@ -93,7 +93,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
   is.clear(); /* clears the end-of-file and error flags */
   is.seekg(0, ios::beg);
 
-  bool edgesOk=true;
+  //bool edgesOk=true;
   while(is){
     char buf[LINESIZE];
     is.getline(buf,LINESIZE);
@@ -119,7 +119,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
 	if (!addEdge(v1, v2,t ,m)){
 	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
 	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
-	  edgesOk=false;
+	  //edgesOk=false;
 	} 
       }
     } else {
@@ -140,7 +140,7 @@ bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool t
 	if (!addEdge(v1, v2,t ,m)){
 	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
 	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
-	  edgesOk=false;
+	  //edgesOk=false;
 	}
       }
     }

corelib/src/toro3d/transformation2.hh

@@ -0,0 +1,410 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file transformation2.hh
+ * \brief Definition of the 2d transformations.
+ *
+ * Definition of the 2d transformations, the symmetrix matrix operations,
+ * handling covariance, etc.
+ **/
+
+#ifndef _TRANSFORMATION2_HXX_
+#define _TRANSFORMATION2_HXX_
+
+#include <cmath>
+
+namespace AISNavigation
+{
+
+/** \brief Template class for representing a 2D point (x and y coordinate) **/
+template <class T>
+struct Vector2{
+  T values[2] ; ///< container for x and y
+
+  /** Constructor **/
+  Vector2(T x, T y)        {values[0]=x; values[1]=y;}
+  /** Default constructor which sets x and y to 0 **/
+  Vector2()                {values[0]=0; values[1]=0;}
+
+  /** @returns Const reference to x **/
+  inline const T& x()      const  {return values[0];}
+  /** @returns Const reference to y **/
+  inline const T& y()      const  {return values[1];}
+
+  /** @returns Reference to x **/
+  inline T& x()            {return values[0];}
+  /** @returns Reference to y **/
+  inline T& y()            {return values[1];}
+
+  /** @returns Norm of the vector **/
+  inline T norm2() const   {
+    return values[0]*values[0]+values[1]*values[1];
+  }
+
+};
+
+/** Operator for scalar multiplication.  **/
+template <class T>
+inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
+  return Vector2<T>(v.values[0]*d, v.values[1]*d);
+}
+
+/** Operator for scalar multiplication.  **/
+template <class T>
+inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
+  return Vector2<T>(v.values[0]*d, v.values[1]*d);
+}
+
+/** Operator for dot product.  **/
+template <class T>
+inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
+  return v1.values[0]*v2.values[0] 
+    + v1.values[1]*v2.values[1];
+}
+
+/** Operator for vector addition.  **/
+template <class T>
+inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
+  return Vector2<T>(v1.values[0]+v2.values[0], 
+		    v1.values[1]+v2.values[1]);
+}
+
+/** Operator for vector subtraction.  **/
+template <class T>
+Vector2<T> operator - (const Vector2<T>& v1, const Vector2<T>& v2){
+  return Vector2<T>(v1.values[0]-v2.values[0], 
+		    v1.values[1]-v2.values[1]);
+}
+
+
+/** \brief 2D Point (x,y) with orientation (theta)
+ *
+ * Tenmplate class for representing a 2D Ooint with x and y
+ * coordinates and an orientation theta in the x-y-plane (theta=0 ->
+ * orientation along the x axis).
+**/
+template <class T>
+struct Pose2{
+  T values[3];///< container for x, y, and theta
+
+  /** @returns Const refernce to x **/
+  inline const T& x()     const  {return values[0];}
+  /** @returns Const refernce to y **/
+  inline const T& y()     const  {return values[1];}
+  /** @returns Const refernce to theta **/
+  inline const T& theta() const  {return values[2];}
+
+  /** @returns Refernce to x **/
+  inline T& x()            {return values[0];}
+  /** @returns Refernce to y **/
+  inline T& y()            {return values[1];}
+  /** @returns Refernce to theta **/
+  inline T& theta()        {return values[2];}
+
+  /** Default constructor which sets x, y, and theta to 0 **/
+  Pose2(){
+    values[0]=0.; values[1]=0.;  values[2]=0.;
+  }
+
+  /** Constructor **/
+  Pose2(const T& x, const T& y, const T& theta){
+    values[0]=x, values[1]=y, values[2]=theta;
+  }
+};
+
+/** Operator for scalar multiplication with a pose **/
+template <class T>
+Pose2<T> operator * (const Pose2<T>& v, const T& d){
+  Pose2<T> r;
+  for (int i=0; i<3; i++){
+    r.values[i]=v.values[i]*d;
+  }
+  return r;
+}
+
+
+/** \brief A class to represent 2D transformations (rotation and translation) **/
+template <class T>
+struct Transformation2{
+  T rotationMatrix[2][2]; ///< the rotation matrix
+  T translationVector[2]; ///< the translation vector
+
+  /** Default constructor 
+   * @param initAsIdentity if true (default) the transormation 
+   *        is the identity, otherwise no initializtion **/
+  Transformation2(bool initAsIdentity = true){
+    if (initAsIdentity) {
+      rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
+      rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
+      translationVector[0]=0.;
+      translationVector[1]=0.;
+    }
+  }
+
+  /** @returns Identity transformation **/
+  inline static Transformation2<T> identity(){
+    Transformation2<T> m(true);
+    return m;
+  }
+
+  /** Constructor that sets the translation and rotation  **/
+  Transformation2 (const T& x, const T& y, const T& theta){
+    setRotation(theta);
+    setTranslation(x,y);
+  }
+
+  /** Constructor that sets the translation and rotation  **/
+  Transformation2 (const T& _theta, const Vector2<T>& trans):
+    Transformation2(trans.x(), trans.y(), _theta){}
+
+
+  /** Copy constructor  **/
+  Transformation2 (const Pose2<T>& v){
+    setRotation(v.theta());
+    setTranslation(v.x(),v.y());
+  }
+
+
+  /** Get the translation  **/
+  inline Vector2<T> translation() const {
+    return Vector2<T>(translationVector[0], 
+		      translationVector[1]);
+  }
+  
+  /** Get the rotation  **/
+  inline T rotation()    const {
+    return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
+  }
+  
+  /** Computed the Pose based on the translation and rotation  **/
+  inline Pose2<T> toPoseType()   const {
+    Vector2<T> t=translation();
+    T r=rotation();
+    Pose2<T> rv(t.x(), t.y(), r );
+    return rv;
+  }
+  
+  /** Set the translation  **/
+  inline void setTranslation(const Vector2<T>& t){
+    setTranslation(t.x(),t.y());
+  }
+
+  /** Set the rotation  **/
+  inline void setRotation(const T& theta){
+    T s=sin(theta), c=cos(theta);
+    rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
+    rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
+  }
+
+  /** Set the translation  **/
+  inline void setTranslation(const T& x, const T& y){
+    translationVector[0]=x;
+    translationVector[1]=y;
+  }
+
+  /** Computes the inveres of the transformation  **/
+  inline Transformation2<T> inv() const {
+    Transformation2<T> rv(*this);
+    for (int i=0; i<2; i++)
+      for (int j=0; j<2; j++){
+	rv.rotationMatrix[i][j]=rotationMatrix[j][i];
+      }
+
+    for (int i=0; i<2; i++){
+      rv.translationVector[i]=0;
+      for (int j=0; j<2; j++){
+	rv.translationVector[i]-=rv.rotationMatrix[i][j]*translationVector[j];
+      }
+    }
+    return rv;
+  }
+
+};
+
+/** Operator for transforming a Vector2 **/
+template <class T>
+Vector2<T> operator * (const Transformation2<T>& m, const Vector2<T>& v){
+  return Vector2<T>(
+		    m.rotationMatrix[0][0]*v.values[0]+
+		    m.rotationMatrix[0][1]*v.values[1]+
+		    m.translationVector[0],
+		    m.rotationMatrix[1][0]*v.values[0]+
+		    m.rotationMatrix[1][1]*v.values[1]+
+		    m.translationVector[1]);
+}
+
+/** Operator for concatenating two transformations **/
+template <class T>
+Transformation2<T> operator * (const Transformation2<T>& m1, const Transformation2<T>& m2){
+  Transformation2<T> rt;
+  for (int i=0; i<2; i++)
+    for (int j=0; j<2; j++){
+      rt.rotationMatrix[i][j]=0.;
+      for (int k=0; k<2; k++)
+	rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
+    }
+  for (int i=0; i<2; i++){
+    rt.translationVector[i]=m1.translationVector[i];
+    for (int j=0; j<2; j++)
+      rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
+  }
+  return rt;
+}
+
+
+/** \brief A class to represent symmetric 3x3 matrices **/
+template <class T>
+struct SMatrix3{
+  T values[3][3];
+  T det() const;
+  SMatrix3<T> transpose() const;
+  SMatrix3<T> adj() const;
+  SMatrix3<T> inv() const;
+};
+
+
+/** Operator for symmetric matrix-pose multiplication **/
+template <class T>
+Pose2<T> operator * (const SMatrix3<T>& m, const Pose2<T>& p){
+  Pose2<T> v;
+  for (int i=0; i<3; i++){
+    v.values[i]=0.;
+    for (int j=0; j<3; j++)
+      v.values[i]+=m.values[i][j]*p.values[j];
+  }
+  return v;
+}
+
+/** Operator for symmetric matrix-scalar multiplication **/
+template <class T>
+SMatrix3<T> operator * (const SMatrix3<T>& s, T& d){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++)
+      m.values[i][j]=d*s.values[i][j];
+  return m;
+}
+
+/** Operator forsymmetric  matrix-symmetric matrix multiplication **/
+template <class T>
+SMatrix3<T> operator * (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++){
+      m.values[i][j]=0.;
+      for (int k=0; k<3; k++){
+	m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
+      }
+    }
+  return m;
+}
+
+/** Operator for symmetric matrix-symmetric matrix addition **/
+template <class T>
+SMatrix3<T> operator + (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++){
+      m.values[i][j]=s1.values[i][j]+s2.values[i][j];
+    }
+  return m;
+}
+
+
+/** Computes the determinat of the symmetric matrix **/
+template <class T>
+T SMatrix3<T>::det() const{
+   T dp= values[0][0]*values[1][1]*values[2][2]
+     +values[0][1]*values[1][2]*values[2][0]
+     +values[0][2]*values[1][0]*values[2][1];
+   T dm=values[2][0]*values[1][1]*values[0][2]
+     +values[2][1]*values[1][2]*values[0][0]
+     +values[2][2]*values[1][0]*values[0][1];
+   return dp-dm;
+}
+
+/** Computes the transposed symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::transpose() const{
+   SMatrix3<T> m;
+   for (int i=0; i<3; i++)
+     for (int j=0; j<3; j++)
+       m.values[j][i]=values[i][j];
+   return m;
+}
+
+/** Computes the complement of the symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::adj() const{
+   SMatrix3<T> m;
+   m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
+   m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
+   m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
+   m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
+   m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
+   m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
+   m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
+   m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
+   m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
+   return m;
+}
+
+/** Computes the inverse (=transposed) symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::inv() const{
+   T id=1./det();
+   SMatrix3<T> i=adj().transpose();
+   return i*id;
+}
+
+
+
+/** \brief Tenmplate class to define the operations in 2D  **/
+template <class T>
+struct Operations2D{
+  typedef T                  BaseType;            /**< base type of the operation typedef **/
+  typedef Pose2<T>           PoseType;            /**< plain representation of the 2d pose as x,y,theta **/
+  typedef Pose2<T>           ParametersType;      /**< plain representation of the 2d pose as x,y,theta **/
+  typedef T                  RotationType;        /**< plain representation of the angle **/
+  typedef Vector2<T>         TranslationType;     /**< plain representation of the 2D translation (x,y) **/
+  typedef Transformation2<T> TransformationType;  /**< homogeneous based representation for a 2d pose, as rotation matrix + vector **/
+  typedef SMatrix3<T>        CovarianceType;      /**< 3 by 3 symmetric covariance matrix for the 2D case **/
+  typedef SMatrix3<T>        InformationType;     /**< 3 by 3 symmetric information matrix for the 2D case **/
+};
+
+} // namespace AISNavigation
+
+#endif

corelib/src/toro3d/treeoptimizer2.cpp

@@ -0,0 +1,367 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file treeoptimizer2.cpp
+ *
+ * \brief Defines the core optimizer class for 2D graphs which is a
+ * subclass of TreePoseGraph2
+ *
+ **/
+
+#include "treeoptimizer2.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+typedef unsigned int uint;
+using namespace std;
+
+namespace AISNavigation {
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+/** \brief A class (struct) to compute the parameterization of the vertex v **/
+struct ParameterPropagator{
+  void perform(TreePoseGraph2::Vertex* v){
+    if (!v->parent){
+      v->parameters=TreePoseGraph2::Pose(0.,0.,0.);
+      return;
+    }
+    v->parameters=TreePoseGraph2::Pose(v->pose.x()-v->parent->pose.x(), 
+		      v->pose.y()-v->parent->pose.y(),
+		      v->pose.theta()-v->parent->pose.theta());
+  }
+};
+
+TreeOptimizer2::TreeOptimizer2(){
+  sortedEdges=0;
+}
+
+TreeOptimizer2::~TreeOptimizer2(){
+}
+
+void TreeOptimizer2::initializeTreeParameters(){
+  ParameterPropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+void TreeOptimizer2::initializeOptimization(){
+  // compute the size of the preconditioning matrix
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+
+  // sorting edges
+  if (sortedEdges!=0){
+    delete sortedEdges;
+    sortedEdges=0;
+  }
+  sortedEdges=sortEdges();
+}
+
+void TreeOptimizer2::initializeOnlineOptimization(){
+  // compute the size of the preconditioning matrix
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+}
+
+void TreeOptimizer2::computePreconditioner(){
+  gamma[0] = gamma[1] =  gamma[2] = numeric_limits<double>::max();
+
+  for (uint i=0; i<M.size(); i++)
+    M[i]=Pose(0.,0.,0.);
+
+  int edgeCount=0;
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    if (! (edgeCount%10000))
+      DEBUG(1) << "m";
+
+    Edge* e=*it;
+    Transformation t=e->transformation;
+    InformationMatrix S=e->informationMatrix;
+    
+    InformationMatrix R;
+    R.values[0][0]=t.rotationMatrix[0][0];
+    R.values[0][1]=t.rotationMatrix[0][1];
+    R.values[0][2]=0;
+    
+    R.values[1][0]=t.rotationMatrix[1][0];
+    R.values[1][1]=t.rotationMatrix[1][1];
+    R.values[1][2]=0;
+    
+    R.values[2][0]=0;
+    R.values[2][1]=0;
+    R.values[2][2]=1;
+
+    InformationMatrix W =R*S*R.transpose();
+    
+    Vertex* top=e->top;
+    for (int dir=0; dir<2; dir++){
+      Vertex* n = (dir==0)? e->v1 : e->v2;
+      while (n!=top){
+	uint i=n->id;
+	M[i].values[0]+=W.values[0][0];
+	M[i].values[1]+=W.values[1][1];
+	M[i].values[2]+=W.values[2][2];
+	gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
+	gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
+	gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
+	n=n->parent;
+      }
+    }
+  }
+  
+  if (verboseLevel>1){
+    for (uint i=0; i<M.size(); i++){
+      cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
+    }
+  }
+}
+
+
+void TreeOptimizer2::propagateErrors(){
+  iteration++;
+  int edgeCount=0;
+  
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+      edgeCount++;
+      if (! (edgeCount%10000)) 	DEBUG(1) << "c";
+      
+      Edge* e=*it;
+      Vertex* top=e->top;
+      
+
+      Vertex* v1=e->v1;
+      Vertex* v2=e->v2;
+
+      double l=e->length;
+      DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+      
+      Pose p1=getPose(v1, top);
+      Pose p2=getPose(v2, top);
+
+      DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+      DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+      
+      Transformation et=e->transformation;
+      Transformation t1(p1);
+      Transformation t2(p2);
+
+      Transformation t12=t1*et;
+    
+      Pose p12=t12.toPoseType();
+      DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+
+      Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
+      double angle=r.theta();
+      angle=atan2(sin(angle),cos(angle));
+      r.theta()=angle;
+      DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+    
+      InformationMatrix S=e->informationMatrix;
+      InformationMatrix R;
+      R.values[0][0]=t1.rotationMatrix[0][0];
+      R.values[0][1]=t1.rotationMatrix[0][1];
+      R.values[0][2]=0;
+      
+      R.values[1][0]=t1.rotationMatrix[1][0];
+      R.values[1][1]=t1.rotationMatrix[1][1];
+      R.values[1][2]=0;
+      
+      R.values[2][0]=0;
+      R.values[2][1]=0;
+      R.values[2][2]=1;
+
+      InformationMatrix W=R*S*R.transpose();
+      Pose d=W*r*2.;
+
+      DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
+    
+      assert(l>0);
+
+      double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
+
+      double tw[3]={0.,0.,0.};
+      for (int dir=0; dir<2; dir++) {
+	Vertex* n = (dir==0)? v1 : v2;
+	while (n!=top){
+	  uint i=n->id;
+	  tw[0]+=1./M[i].values[0];
+	  tw[1]+=1./M[i].values[1];
+	  tw[2]+=1./M[i].values[2];
+	  n=n->parent;
+	}
+      }
+
+      double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
+      beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
+      beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
+      beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
+
+      DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
+      DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
+      
+      for (int dir=0; dir<2; dir++) {
+	Vertex* n = (dir==0)? v1 : v2;
+	double sign=(dir==0)? -1. : 1.;
+	while (n!=top){
+	  uint i=n->id;
+	  assert(M[i].values[0]>0);
+	  assert(M[i].values[1]>0);
+	  assert(M[i].values[2]>0);
+
+	  Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
+	  delta=delta*sign;
+	  DEBUG(2) << "   " << dir << ":"  << i <<"," << n->parent->id << ":"
+	       << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
+
+	  n->parameters.x()+=delta.x();
+	  n->parameters.y()+=delta.y();
+	  n->parameters.theta()+=delta.theta();
+	  DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
+	  n=n->parent;
+	}
+      }
+      updatePoseChain(v1,top);
+      updatePoseChain(v2,top);
+
+      Pose pf1=v1->pose;
+      Pose pf2=v2->pose;
+      
+      DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
+      DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
+      DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
+    }
+  
+}
+
+void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
+  TreePoseGraph2::EdgeSet* temp=sortedEdges;
+  if (eset){
+    sortedEdges=eset;
+  }
+  computePreconditioner();
+  propagateErrors();
+  sortedEdges=temp;
+}
+
+void TreeOptimizer2::updatePoseChain(Vertex* v, Vertex* top){
+  if (v!=top){
+    updatePoseChain(v->parent, top);
+    v->pose.x()=v->parent->pose.x()+v->parameters.x();
+    v->pose.y()=v->parent->pose.y()+v->parameters.y();
+    v->pose.theta()=v->parent->pose.theta()+v->parameters.theta();
+    return;
+  } 
+}
+
+TreeOptimizer2::Pose TreeOptimizer2::getPose(Vertex*v, Vertex* top){
+  Pose p(0,0,0);
+  Vertex* aux=v;
+  while (aux!=top){
+    p.x()+=aux->parameters.x();
+    p.y()+=aux->parameters.y();
+    p.theta()+=aux->parameters.theta();
+    aux=aux->parent;
+  }
+  p.x()+=aux->pose.x();
+  p.y()+=aux->pose.y();
+  p.theta()+=aux->pose.theta();
+  return p;
+}
+
+
+double TreeOptimizer2::error(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Pose p1=v1->pose;
+  Pose p2=v2->pose;
+  
+  DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+  DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+  
+  Transformation et=e->transformation;
+  Transformation t1(p1);
+  Transformation t2(p2);
+  
+  Transformation t12=t1*et;
+    
+  Pose p12=t12.toPoseType();
+  DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+
+  Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
+  double angle=r.theta();
+  angle=atan2(sin(angle),cos(angle));
+  r.theta()=angle;
+  DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+    
+  InformationMatrix S=e->informationMatrix;
+  InformationMatrix R;
+  R.values[0][0]=t1.rotationMatrix[0][0];
+  R.values[0][1]=t1.rotationMatrix[0][1];
+  R.values[0][2]=0;
+      
+  R.values[1][0]=t1.rotationMatrix[1][0];
+  R.values[1][1]=t1.rotationMatrix[1][1];
+  R.values[1][2]=0;
+      
+  R.values[2][0]=0;
+  R.values[2][1]=0;
+  R.values[2][2]=1;
+
+  InformationMatrix W=R*S*R.transpose();
+
+  Pose r1=W*r;
+  return r.x()*r1.x()+r.y()*r1.y()+r.theta()*r1.theta();
+}
+
+double TreeOptimizer2::error() const{
+  double globalError=0.;
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    globalError+=error(it->second);
+  }
+  return globalError;
+}
+
+}; //namespace AISNavigation

corelib/src/toro3d/treeoptimizer2.hh

@@ -0,0 +1,107 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file treeoptimizer2.hh
+ *
+ * \brief Defines the core optimizer class for 2D graphs which is a
+ * subclass of TreePoseGraph2
+ *
+ **/
+
+#ifndef _TREEOPTIMIZER2_HH_
+#define _TREEOPTIMIZER2_HH_
+
+#include "posegraph2.hh"
+
+namespace AISNavigation {
+
+/** \brief Class that contains the core optimization algorithm **/
+struct TreeOptimizer2: public TreePoseGraph2{
+  typedef std::vector<Pose> PoseVector;
+
+  /** Constructor **/
+  TreeOptimizer2();
+
+  /** Destructor **/
+  virtual ~TreeOptimizer2();
+
+  /** Initialization function **/
+  void initializeTreeParameters();
+
+  /** Initialization function **/
+  void initializeOptimization();
+
+  /** Initialization function **/
+  void initializeOnlineOptimization();
+  
+  /** Performs one iteration of the algorithm **/
+  void iterate(TreePoseGraph2::EdgeSet* eset=0);
+
+  /** Conmputes the gloabl error of the network **/
+  double error() const;
+
+protected:
+  /** The first of the two main steps of each iteration **/
+  void computePreconditioner();
+
+  /** The second of the two main steps of each iteration **/
+  void propagateErrors();
+
+  /** Recomputes the poses of all vertices from v to an arbitraty
+      parent (top) of v in the tree **/
+  void updatePoseChain(Vertex* v, Vertex* top);
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Pose getPose(Vertex*v, Vertex* top);
+
+  /** Conmputes the error of the constraint/edge e **/
+  double error(const Edge* e) const;
+
+  /** Iteration counter **/
+  int iteration;
+
+  /** Used to compute the learning rate lambda **/
+  double gamma[3];
+
+  /** The diaginal block elements of the preconditioning matrix (D_k
+      in the paper) **/
+  PoseVector M;
+
+};
+
+}; //namespace AISNavigation
+#endif

corelib/src/toro3d/treeoptimizer3.cpp

@@ -95,7 +95,7 @@ void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditione
     maxRotationalErrors.push_back(mre);
     int interval=3;
     if ((int)maxRotationalErrors.size()>=interval){
-      uint s=maxRotationalErrors.size();
+      uint s=(uint)maxRotationalErrors.size();
       double re0 = maxRotationalErrors[s-interval];
       double re1 = maxRotationalErrors[s-1];
 

examples/BOWMapping/main.cpp

@@ -139,7 +139,7 @@ int main(int argc, char * argv[])
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
 					rtabmap.getLoopClosureId(),
-					rtabmap.getLcHypValue(),
+					rtabmap.getLoopClosureValue(),
 					nextIndex,
 					rtabmap.getLoopClosureId());
 		}
@@ -150,8 +150,8 @@ int main(int argc, char * argv[])
 					rtabmap.getLastProcessTime(),
 					(int)rtabmap.getSTM().size(), // short-term memory
 					(int)rtabmap.getWM().size(), // working memory
-					rtabmap.getRetrievedId(), // highest loop closure hypothesis
+					rtabmap.getHighestHypothesisId(), // highest loop closure hypothesis
-					rtabmap.getLcHypValue());
+					rtabmap.getLoopClosureValue());
 		}
 
 		++nextIndex;

examples/CMakeLists.txt

@@ -2,9 +2,10 @@
 ADD_SUBDIRECTORY( BOWMapping )
 
 IF(TARGET rtabmap_gui)
-	ADD_SUBDIRECTORY( RGBDMapping )
+   ADD_SUBDIRECTORY( RGBDMapping )
+   ADD_SUBDIRECTORY( WifiMapping )
 ELSE()
-   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping example will not be built...")
+   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
 ENDIF()
 
 

examples/RGBDMapping/CMakeLists.txt

@@ -7,7 +7,9 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
-INCLUDE(${QT_USE_FILE})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES}
@@ -17,7 +19,11 @@ SET(LIBRARIES
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
-QT4_WRAP_CPP(moc_srcs MapBuilder.h)
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs MapBuilder.h)
+ELSE()
+    QT5_WRAP_CPP(moc_srcs MapBuilder.h)
+ENDIF()
 
 ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs})
   

examples/RGBDMapping/MapBuilder.h

@@ -28,16 +28,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef MAPBUILDER_H_
 #define MAPBUILDER_H_
 
-#include <QtGui/QVBoxLayout>
+#include <QVBoxLayout>
 #include <QtCore/QMetaType>
+#include <QAction>
+
+#ifndef Q_MOC_RUN // Mac OS X issue
 #include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#endif
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/core/util3d.h"
-#include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/CameraThread.h"
 
 using namespace rtabmap;
 
@@ -46,9 +51,12 @@ class MapBuilder : public QWidget, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	MapBuilder() :
+	//Camera ownership is not transferred!
-		_processingStatistics(false),
+	MapBuilder(CameraThread * camera = 0) :
-		_lastOdometryProcessed(true)
+		camera_(camera),
+		odometryCorrection_(Transform::getIdentity()),
+		processingStatistics_(false),
+		lastOdometryProcessed_(true)
 	{
 		this->setWindowFlags(Qt::Dialog);
 		this->setWindowTitle(tr("3D Map"));
@@ -63,6 +71,11 @@ public:
 
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
 		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
 	}
 
 	virtual ~MapBuilder()
@@ -70,8 +83,24 @@ public:
 		this->unregisterFromEventsManager();
 	}
 
-private slots:
+protected slots:
-	void processOdometry(const rtabmap::SensorData & data)
+	virtual void pauseDetection()
+	{
+		UWARN("");
+		if(camera_)
+		{
+			if(camera_->isCapturing())
+			{
+				camera_->join(true);
+			}
+			else
+			{
+				camera_->start();
+			}
+		}
+	}
+
+	virtual void processOdometry(const rtabmap::SensorData & data)
 	{
 		if(!this->isVisible())
 		{
@@ -88,7 +117,7 @@ private slots:
 		}
 		else
 		{
-			cloudViewer_->setBackgroundColor(Qt::black);
+			cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
 		}
 		if(!pose.isNull())
 		{
@@ -117,7 +146,7 @@ private slots:
 						cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
 					}
 				}
-				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, pose))
+				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
 				{
 					UERROR("Adding cloudOdom to viewer failed!");
 				}
@@ -126,19 +155,22 @@ private slots:
 			if(!data.pose().isNull())
 			{
 				// update camera position
-				cloudViewer_->updateCameraPosition(data.pose());
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*data.pose());
 			}
 		}
-		cloudViewer_->render();
+		cloudViewer_->update();
 
-		_lastOdometryProcessed = true;
+		lastOdometryProcessed_ = true;
 	}
 
 
-	void processStatistics(const rtabmap::Statistics & stats)
+	virtual void processStatistics(const rtabmap::Statistics & stats)
 	{
-		_processingStatistics = true;
+		processingStatistics_ = true;
 
+		//============================
+		// Add RGB-D clouds
+		//============================
 		const std::map<int, Transform> & poses = stats.poses();
 		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -165,15 +197,17 @@ private slots:
 				else if(iter->first == stats.refImageId() &&
 						stats.getSignature().id() == iter->first)
 				{
+					Signature s = stats.getSignature();
+					s.uncompressData(); // make sure data is uncompressed
 					// Add the new cloud
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
-							stats.getSignature().getImageRaw(),
+							s.getImageRaw(),
-							stats.getSignature().getDepthRaw(),
+							s.getDepthRaw(),
-							stats.getSignature().getDepthCx(),
+							s.getCx(),
-							stats.getSignature().getDepthCy(),
+							s.getCy(),
-							stats.getSignature().getDepthFx(),
+							s.getFx(),
-							stats.getSignature().getDepthFy(),
+							s.getFy(),
-						   8); // decimation
+						   4); // decimation
 
 					if(cloud->size())
 					{
@@ -191,12 +225,36 @@ private slots:
 			}
 		}
 
-		cloudViewer_->render();
+		//============================
+		// Add 3D graph (show all poses)
+		//============================
+		cloudViewer_->removeAllGraphs();
+		cloudViewer_->removeCloud("graph_nodes");
+		if(poses.size())
+		{
+			// Set graph
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+			}
+			*graphNodes = *graph;
 
-		_processingStatistics = false;
+
+			// add graph
+			cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
+			cloudViewer_->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+			cloudViewer_->setCloudPointSize("graph_nodes", 5);
+		}
+
+		odometryCorrection_ = stats.mapCorrection();
+
+		cloudViewer_->update();
+
+		processingStatistics_ = false;
 	}
 
-protected:
 	virtual void handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("RtabmapEvent") == 0)
@@ -214,20 +272,22 @@ protected:
 			OdometryEvent * odomEvent = (OdometryEvent *)event;
 			// Odometry must be processed in the Qt thread
 			if(this->isVisible() &&
-			   _lastOdometryProcessed &&
+			   lastOdometryProcessed_ &&
-			   !_processingStatistics)
+			   !processingStatistics_)
 			{
-				_lastOdometryProcessed = false; // if we receive too many odometry events!
+				lastOdometryProcessed_ = false; // if we receive too many odometry events!
 				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
 			}
 		}
 	}
 
-private:
+protected:
 	CloudViewer * cloudViewer_;
+	CameraThread * camera_;
 	Transform lastOdomPose_;
-	bool _processingStatistics;
+	Transform odometryCorrection_;
-	bool _lastOdometryProcessed;
+	bool processingStatistics_;
+	bool lastOdometryProcessed_;
 };
 
 

examples/RGBDMapping/main.cpp

@@ -30,8 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/utilite/UEventsManager.h"
-#include <QtGui/QApplication>
+#include <QApplication>
 #include <stdio.h>
 
 #include "MapBuilder.h"
@@ -65,10 +66,6 @@ int main(int argc, char * argv[])
 		}
 	}
 
-	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
-	QApplication app(argc, argv);
-	MapBuilder mapBuilder;
-
 	// Here is the pipeline that we will use:
 	// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
 
@@ -83,7 +80,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new CameraOpenNI2(0, opticalRotation);
+		camera = new CameraOpenNI2("", 0, opticalRotation);
 	}
 	else if(driver == 2)
 	{
@@ -124,6 +121,11 @@ int main(int argc, char * argv[])
 		exit(1);
 	}
 
+	// 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);
+	MapBuilder mapBuilder(&cameraThread);
+
 	// Create an odometry thread to process camera events, it will send OdometryEvent.
 	OdometryThread odomThread(new OdometryBOW());
 

examples/WifiMapping/CMakeLists.txt

@@ -0,0 +1,50 @@
+
+SET(srcs
+    main.cpp)
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES}
+	${QT_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+ELSE()
+    QT5_WRAP_CPP(moc_srcs ../RGBDMapping/MapBuilder.h MapBuilderWifi.h)
+ENDIF()
+
+IF(APPLE)
+    FIND_LIBRARY(CoreWLAN_LIBRARY CoreWLAN)
+    FIND_LIBRARY(Foundation_LIBRARY Foundation)
+    MARK_AS_ADVANCED(CoreWLAN_LIBRARY Foundation_LIBRARY)
+    SET(LIBRARIES
+	    ${LIBRARIES}
+	    ${CoreWLAN_LIBRARY}
+	    ${Foundation_LIBRARY}
+	)
+	SET(srcs
+	    ${srcs}
+	    WifiOSX.mm
+	)
+ENDIF(APPLE)
+
+ADD_EXECUTABLE(wifi_mapping ${srcs} ${moc_srcs})
+TARGET_LINK_LIBRARIES(wifi_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( wifi_mapping 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-wifi_mapping)

examples/WifiMapping/MapBuilderWifi.h

@@ -0,0 +1,193 @@
+/*
+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 MAPBUILDERWIFI_H_
+#define MAPBUILDERWIFI_H_
+
+#include "../RGBDMapping/MapBuilder.h"
+
+using namespace rtabmap;
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int dBm2Quality(int dBm)
+{
+	// dBm to Quality:
+    if(dBm <= -100)
+        return 0;
+    else if(dBm >= -50)
+    	return 100;
+    else
+    	return 2 * (dBm + 100);
+}
+
+class MapBuilderWifi : public MapBuilder
+{
+	Q_OBJECT
+public:
+	// Camera ownership is not transferred!
+	MapBuilderWifi(CameraThread * camera = 0) :
+		MapBuilder(camera)
+	{}
+
+	virtual ~MapBuilderWifi()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+protected slots:
+	virtual void processStatistics(const rtabmap::Statistics & stats)
+	{
+		processingStatistics_ = true;
+
+		const std::map<int, Transform> & poses = stats.poses();
+		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
+
+		//============================
+		// 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)
+		{
+			// 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)));
+			}
+		}
+
+		for(std::map<int, std::pair<int, double> >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++iter)
+		{
+			// The Wifi value may be taken between two nodes, interpolate its position.
+			double stampWifi = iter->second.second;
+			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
+
+			if(previousNode != nodeStamps.end() && nextNode != nodeStamps.end() &&
+			   previousNode->second != nextNode->second &&
+			   uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
+			{
+				Transform poseA = poses.at(previousNode->second);
+				Transform poseB = poses.at(nextNode->second);
+				double stampA = previousNode->first;
+				double stampB = nextNode->first;
+				UASSERT(stampWifi>=stampA && stampWifi <=stampB);
+
+				Transform v = poseA.inverse() * poseB;
+				double ratio = (stampWifi-stampA)/(stampB-stampA);
+
+				v.x()*=ratio;
+				v.y()*=ratio;
+				v.z()*=ratio;
+
+				Transform wifiPose = (poseA*v).translation(); // rip off the rotation
+
+				std::string cloudName = uFormat("level%d", iter->first);
+				if(clouds.contains(cloudName))
+				{
+					if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
+					{
+						UERROR("Updating pose cloud %d failed!", iter->first);
+					}
+				}
+				else
+				{
+					// Make a line with points
+					int quality = dBm2Quality(iter->second.first)/10;
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+					for(int i=0; i<10; ++i)
+					{
+						// 2 cm between each points
+						// the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
+						pcl::PointXYZRGB pt;
+						pt.z = float(i+1)*0.02f;
+						if(i<quality)
+						{
+							// yellow
+							pt.r = 255;
+							pt.g = 255;
+						}
+						else
+						{
+							// gray
+							pt.r = pt.g = pt.b = 100;
+						}
+						cloud->push_back(pt);
+					}
+					pcl::PointXYZRGB anchor(255, 0, 0);
+					cloud->push_back(anchor);
+					//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);
+					}
+					else
+					{
+						cloudViewer_->setCloudPointSize(cloudName, 5);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Bounds not found!");
+			}
+		}
+
+		//============================
+		// Add RGB-D clouds
+		//============================
+		MapBuilder::processStatistics(stats);
+	}
+};
+
+
+#endif /* MAPBUILDERWIFI_H_ */

examples/WifiMapping/WifiOSX.h

@@ -0,0 +1,25 @@
+/*
+ * WifiOSX.h
+ *
+ *  Created on: Mar 26, 2015
+ *      Author: mathieu
+ */
+
+#ifndef WIFIOSX_H_
+#define WIFIOSX_H_
+
+#include <string>
+#include <vector>
+
+struct AccessPoint
+{
+    std::string ssid;
+    std::string bssid;
+    int rssi;
+};
+
+int getRssi(const std::string& interfaceName);
+std::vector<AccessPoint> scanAir(const std::string& interfaceName);
+
+
+#endif /* WIFIOSX_H_ */

examples/WifiMapping/WifiOSX.mm

@@ -0,0 +1,34 @@
+#import <CoreWLAN/CoreWLAN.h>
+#include "WifiOSX.h"
+
+int getRssi(const std::string& interfaceName)
+{
+    NSString* ifName = [NSString stringWithUTF8String:interfaceName.c_str()];
+    CWInterface* interface = [CWInterface interfaceWithName:ifName];
+    return interface.rssiValue;
+}
+
+std::vector<AccessPoint> scanAir(const std::string& interfaceName)
+{
+    NSString* ifName = [NSString stringWithUTF8String:interfaceName.c_str()];
+    CWInterface* interface = [CWInterface interfaceWithName:ifName];
+    
+    NSError* error = nil;
+    NSArray* scanResult = [[interface scanForNetworksWithSSID:nil error:&error] allObjects];
+    if (error)
+    {
+        NSLog(@"%@ (%ld)", [error localizedDescription], [error code]);
+    }
+    
+    std::vector<AccessPoint> result;
+    for (CWNetwork* network in scanResult)
+    {
+        AccessPoint ap;
+        ap.ssid  = std::string([[network ssid] UTF8String]);
+        ap.bssid = std::string([[network bssid] UTF8String]);
+        ap.rssi = [network rssiValue];
+        result.push_back(ap);
+    }
+    
+    return result;
+}

examples/WifiMapping/WifiThread.h

@@ -0,0 +1,225 @@
+/*
+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 WIFITHREAD_H_
+#define WIFITHREAD_H_
+
+#ifdef _WIN32
+	#ifndef UNICODE
+	#define UNICODE
+	#endif
+
+	#include <windows.h>
+	#include <wlanapi.h>
+	#include <Windot11.h>           // for DOT11_SSID struct
+	#include <objbase.h>
+	#include <wtypes.h>
+
+	#include <stdio.h>
+	#include <stdlib.h>
+
+	// Need to link with Wlanapi.lib and Ole32.lib
+	#pragma comment(lib, "wlanapi.lib")
+	#pragma comment(lib, "ole32.lib")
+#elif __APPLE__
+#include "WifiOSX.h"
+#else
+	#include <sys/socket.h>
+	#include <linux/wireless.h>
+	#include <sys/ioctl.h>
+#endif
+#include <rtabmap/core/UserDataEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+
+// A percentage value that represents the signal quality
+// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
+// This member contains a value between 0 and 100. A value
+// of 0 implies an actual RSSI signal strength of -100 dbm.
+// A value of 100 implies an actual RSSI signal strength of -50 dbm.
+// You can calculate the RSSI signal strength value for wlanSignalQuality
+// values between 1 and 99 using linear interpolation.
+inline int quality2dBm(int quality)
+{
+	// Quality to dBm:
+	if(quality <= 0)
+		return -100;
+	else if(quality >= 100)
+		return -50;
+	else
+		return (quality / 2) - 100;
+}
+
+
+class WifiThread : public UThread, public UEventsSender
+{
+public:
+	WifiThread(const std::string & interfaceName, float rate = 0.5) :
+		interfaceName_(interfaceName),
+		rate_(rate)
+	{}
+	virtual ~WifiThread() {}
+
+private:
+	virtual void mainLoop()
+	{
+		uSleep(1000/rate_);
+		if(!this->isKilled())
+		{
+			int dBm = 0;
+#ifdef _WIN32
+			//From https://msdn.microsoft.com/en-us/library/windows/desktop/ms706765(v=vs.85).aspx
+			// Declare and initialize variables.
+			HANDLE hClient = NULL;
+			DWORD dwMaxClient = 2;      //
+			DWORD dwCurVersion = 0;
+			DWORD dwResult = 0;
+
+			// variables used for WlanEnumInterfaces
+			PWLAN_INTERFACE_INFO_LIST pIfList = NULL;
+			PWLAN_INTERFACE_INFO pIfInfo = NULL;
+
+			// variables used for WlanQueryInterfaces for opcode = wlan_intf_opcode_current_connection
+			PWLAN_CONNECTION_ATTRIBUTES pConnectInfo = NULL;
+			DWORD connectInfoSize = sizeof(WLAN_CONNECTION_ATTRIBUTES);
+			WLAN_OPCODE_VALUE_TYPE opCode = wlan_opcode_value_type_invalid;
+
+			dwResult = WlanOpenHandle(dwMaxClient, NULL, &dwCurVersion, &hClient);
+			if (dwResult != ERROR_SUCCESS)
+			{
+				UERROR("WlanOpenHandle failed with error: %u\n", dwResult);
+			}
+			else
+			{
+				dwResult = WlanEnumInterfaces(hClient, NULL, &pIfList);
+				if (dwResult != ERROR_SUCCESS)
+				{
+					UERROR("WlanEnumInterfaces failed with error: %u\n", dwResult);
+				}
+				else
+				{
+					// take the first interface found
+					int i = 0;
+					pIfInfo = (WLAN_INTERFACE_INFO *) & pIfList->InterfaceInfo[i];
+					if(pIfInfo->isState == wlan_interface_state_connected)
+					{
+						dwResult = WlanQueryInterface(hClient,
+													  &pIfInfo->InterfaceGuid,
+													  wlan_intf_opcode_current_connection,
+													  NULL,
+													  &connectInfoSize,
+													  (PVOID *) &pConnectInfo,
+													  &opCode);
+
+						if (dwResult != ERROR_SUCCESS)
+						{
+							UERROR("WlanQueryInterface failed with error: %u\n", dwResult);
+						}
+						else
+						{
+							int quality = pConnectInfo->wlanAssociationAttributes.wlanSignalQuality;
+							dBm = quality2dBm(quality);
+						}
+					}
+					else
+					{
+						UERROR("Interface not connected!");
+					}
+				}
+			}
+			if (pConnectInfo != NULL)
+			{
+				WlanFreeMemory(pConnectInfo);
+				pConnectInfo = NULL;
+			}
+
+			if (pIfList != NULL)
+			{
+				WlanFreeMemory(pIfList);
+				pIfList = NULL;
+			}
+#elif __APPLE__
+			dBm = getRssi(interfaceName_);
+#else
+			// Code inspired from http://blog.ajhodges.com/2011/10/using-ioctl-to-gather-wifi-information.html
+
+			//have to use a socket for ioctl
+			int sockfd;
+			/* Any old socket will do, and a datagram socket is pretty cheap */
+			if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+				UERROR("Could not create simple datagram socket");
+				return;
+			}
+
+			struct iwreq req;
+			struct iw_statistics stats;
+
+			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ);
+
+			//make room for the iw_statistics object
+			req.u.data.pointer = (caddr_t) &stats;
+			req.u.data.length = sizeof(stats);
+			// clear updated flag
+			req.u.data.flags = 1;
+
+			//this will gather the signal strength
+			if(ioctl(sockfd, SIOCGIWSTATS, &req) == -1)
+			{
+				//die with error, invalid interface
+				UERROR("Invalid interface (\"%s\"). Tip: Try with sudo!", interfaceName_.c_str());
+			}
+			else if(((iw_statistics *)req.u.data.pointer)->qual.updated & IW_QUAL_DBM)
+			{
+				//signal is measured in dBm and is valid for us to use
+				dBm = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
+			}
+			else
+			{
+				UERROR("Could not get signal level.");
+			}
+
+			close(sockfd);
+#endif
+			if(dBm != 0)
+			{
+				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));
+				this->post(new UserDataEvent(data));
+				//UWARN("posting level %d dBm", dBm);
+			}
+		}
+	}
+
+private:
+	std::string interfaceName_;
+	float rate_;
+};
+
+#endif /* WIFITHREAD_H_ */

examples/WifiMapping/main.cpp

@@ -0,0 +1,219 @@
+/*
+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/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <QApplication>
+#include <stdio.h>
+
+#include "MapBuilderWifi.h"
+
+#include "WifiThread.h"
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-wifi_mapping [options]\n"
+			"Options:\n"
+			"  -i \"name\"            Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
+			"  -m                     Enable mirroring of the camera image.\n"
+			"  -d #                   Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+	exit(1);
+}
+
+using namespace rtabmap;
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	std::string interfaceName = "eth0";
+	int driver = 0;
+	bool mirroring = false;
+
+	// parse options
+	for(int i = 1; i<argc; ++i)
+	{
+		if(strcmp(argv[i], "-i") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				interfaceName = argv[i];
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-m") == 0)
+		{
+			mirroring = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-d") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				driver = atoi(argv[i]);
+				if(driver < 0 || driver > 4)
+				{
+					UERROR("driver should be between 0 and 4.");
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+
+		UERROR("Option \"%s\" not recognized!", argv[i]);
+		showUsage();
+	}
+
+	// Here is the pipeline that we will use:
+	// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// 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;
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	if(driver == 1)
+	{
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNI2("", 0, opticalRotation);
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect(0, 0, opticalRotation);
+	}
+	else if(driver == 3)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(false, 0, opticalRotation);
+	}
+	else if(driver == 4)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(true, 0, opticalRotation);
+	}
+	else
+	{
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+	}
+
+	if(mirroring)
+	{
+		camera->setMirroringEnabled(true);
+	}
+
+	CameraThread cameraThread(camera);
+	if(!cameraThread.init())
+	{
+		UERROR("Camera init failed!");
+		//exit(1);
+	}
+
+	// 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);
+	MapBuilderWifi mapBuilderWifi(&cameraThread);
+
+	// Create an odometry thread to process camera events, it will send OdometryEvent.
+	OdometryThread odomThread(new OdometryBOW());
+
+	// Create RTAB-Map to process OdometryEvent
+	Rtabmap * rtabmap = new Rtabmap();
+	ParametersMap param;
+	param.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // disable rehearsal (node merging when not moving)
+	rtabmap->init(param);
+	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
+
+	// Create Wifi monitoring thread
+	WifiThread wifiThread(interfaceName); // 0.5 Hz, should be under RTAB-Map rate (which is 1 Hz by default)
+
+	// Setup handlers
+	odomThread.registerToEventsManager();
+	rtabmapThread.registerToEventsManager();
+	mapBuilderWifi.registerToEventsManager();
+
+	// The RTAB-Map is subscribed by default to CameraEvent, but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.
+	// We can do that by creating a "pipe" between the camera and odometry, then
+	// only the odometry will receive CameraEvent from that camera. RTAB-Map is
+	// also subscribed to OdometryEvent by default, so no need to create a pipe between
+	// odometry and RTAB-Map.
+	UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
+
+	// Let's start the threads
+	rtabmapThread.start();
+	odomThread.start();
+	cameraThread.start();
+	wifiThread.start();
+
+	mapBuilderWifi.show();
+	app.exec(); // main loop
+
+	// remove handlers
+	mapBuilderWifi.unregisterFromEventsManager();
+	rtabmapThread.unregisterFromEventsManager();
+	odomThread.unregisterFromEventsManager();
+
+	// Kill all threads
+	cameraThread.kill();
+	odomThread.join(true);
+	rtabmapThread.join(true);
+	wifiThread.join(true);
+
+	return 0;
+}

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -30,9 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include <QtGui/QDialog>
+#include <QDialog>
+#include <QSettings>
 #include <opencv2/opencv.hpp>
 
+#include <rtabmap/core/CameraModel.h>
+
 #include <rtabmap/utilite/UEventsHandler.h>
 
 class Ui_calibrationDialog;
@@ -44,16 +47,20 @@ class RTABMAPGUI_EXP CalibrationDialog  : public QDialog, public UEventsHandler
 	Q_OBJECT;
 
 public:
-	CalibrationDialog(QWidget * parent = 0);
+	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0);
 	virtual ~CalibrationDialog();
 
-	bool isCalibrated() const {return calibrated_;}
+	bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);}
-	const cv::Mat & cameraMatrix() const {return cameraMatrix_;} // Matrix K
+	const rtabmap::CameraModel & getLeftCameraModel() const {return models_[0];}
-	const cv::Mat & distCoeffs() const {return distCoeffs_;} // Matrix D
+	const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];}
-	float fx() const {return cameraMatrix_.at<double>(0,0);} // K(0)
+	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
-	float fy() const {return cameraMatrix_.at<double>(1,1);} // K(4)
+
-	float cx() const {return cameraMatrix_.at<double>(0,2);} // K(2)
+	void saveSettings(QSettings & settings, const QString & group = "") const;
-	float cy() const {return cameraMatrix_.at<double>(1,2);} // K(5)
+	void loadSettings(QSettings & settings, const QString & group = "");
+
+	void setSwitchedImages(bool switched);
+	void setStereoMode(bool stereo);
+	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 
 public slots:
 	void setBoardWidth(int width);
@@ -61,9 +68,10 @@ public slots:
 	void setSquareSize(double size);
 
 private slots:
-	void processImage(const cv::Mat & image);
+	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
 	void restart();
 	void calibrate();
+	bool save();
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);
@@ -82,15 +90,21 @@ private:
 
 private:
 	// parameters
-	cv::Size boardSize_; // innner squares
+	bool stereo_;
-	float squareSize_; // m
+	QString savingDirectory_;
 
-	std::vector<std::vector<cv::Point2f> > imagePoints_;
+	QString cameraName_;
-	std::vector<std::vector<float> > imageParams_;
+	bool processingData_;
-	cv::Size imageSize_;
+	bool savedCalibration_;
-	bool calibrated_;
+
-	cv::Mat cameraMatrix_;
+	std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
-	cv::Mat distCoeffs_;
+	std::vector<std::vector<std::vector<float> > > imageParams_;
+	std::vector<std::vector<std::vector<cv::Point2f> > > stereoImagePoints_;
+	std::vector<cv::Size > imageSize_;
+	std::vector<rtabmap::CameraModel> models_;
+	rtabmap::StereoCameraModel stereoModel_;
+	std::vector<unsigned short> minIrs_;
+	std::vector<unsigned short> maxIrs_;
 
 	Ui_calibrationDialog * ui_;
 };

guilib/include/rtabmap/gui/CameraViewer.h

@@ -0,0 +1,61 @@
+/*
+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 CAMERAVIEWER_H_
+#define CAMERAVIEWER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QDialog>
+#include <rtabmap/core/SensorData.h>
+
+namespace rtabmap {
+
+class ImageView;
+class CloudViewer;
+
+class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	CameraViewer(QWidget * parent = 0);
+	virtual ~CameraViewer();
+
+public slots:
+	void showImage(const rtabmap::SensorData & data);
+protected:
+	virtual void handleEvent(UEvent * event);
+
+private:
+	ImageView* imageView_;
+	CloudViewer* cloudView_;
+	bool processingImages_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAVIEWER_H_ */

guilib/include/rtabmap/gui/CloudViewer.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QMap>
 #include <QtCore/QSet>
 #include <QtCore/qnamespace.h>
+#include <QtCore/QSettings>
 
 #include <opencv2/opencv.hpp>
 
@@ -63,6 +64,9 @@ public:
 	CloudViewer(QWidget * parent = 0);
 	virtual ~CloudViewer();
 
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
+
 	bool updateCloudPose(
 		const std::string & id,
 		const Transform & pose); //including mesh
@@ -70,43 +74,45 @@ public:
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = QColor());
 
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 			const Transform & pose = Transform::getIdentity(),
-			const QColor & color = Qt::gray);
+			const QColor & color = QColor());
 
 	bool addCloud(
 			const std::string & id,
 			const pcl::PCLPointCloud2Ptr & binaryCloud,
 			const Transform & pose,
 			bool rgb,
-			const QColor & color = Qt::gray);
+			const QColor & color = QColor());
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = Qt::gray);
+		const QColor & color = QColor());
 
 	bool addCloudMesh(
 		const std::string & id,
@@ -127,7 +133,7 @@ public:
 			float opacity);
 	void removeOccupancyGridMap();
 
-	void updateCameraPosition(
+	void updateCameraTargetPosition(
 		const Transform & pose);
 
 	void addOrUpdateGraph(
@@ -137,8 +143,10 @@ public:
 	void removeGraph(const std::string & id);
 	void removeAllGraphs();
 
+	bool isTrajectoryShown() const;
+	unsigned int getTrajectorySize() const;
 	void setTrajectoryShown(bool shown);
-	void setTrajectorySize(int value);
+	void setTrajectorySize(unsigned int value);
 	void clearTrajectory();
 
 	void removeAllClouds(); //including meshes
@@ -148,26 +156,50 @@ public:
 	bool getCloudVisibility(const std::string & id);
 
 	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
+	const QColor & getDefaultBackgroundColor() const;
 	const QColor & getBackgroundColor() const;
+	Transform getTargetPose() const;
+	void getCameraPosition(
+			float & x, float & y, float & z,
+			float & focalX, float & focalY, float & focalZ,
+			float & upX, float & upY, float & upZ) const;
+	bool isCameraTargetLocked() const;
+	bool isCameraTargetFollow() const;
+	bool isCameraFree() const;
+	bool isCameraLockZ() const;
+	bool isGridShown() const;
+	unsigned int getGridCellCount() const;
+	float getGridCellSize() const;
 
+	void setCameraPosition(
+			float x, float y, float z,
+			float focalX, float focalY, float focalZ,
+			float upX, float upY, float upZ);
 	void setCameraTargetLocked(bool enabled = true);
 	void setCameraTargetFollow(bool enabled = true);
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
 	void setGridShown(bool shown);
+	void setGridCellCount(unsigned int count);
+	void setGridCellSize(float size);
 	void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
 
 public slots:
-	void render();
+	void setDefaultBackgroundColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	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();}
 
+signals:
+	void configChanged();
+
 protected:
 	virtual void keyReleaseEvent(QKeyEvent * event);
 	virtual void keyPressEvent(QKeyEvent * event);
+	virtual void mousePressEvent(QMouseEvent * event);
+	virtual void mouseMoveEvent(QMouseEvent * event);
 	virtual void contextMenuEvent(QContextMenuEvent * event);
 	virtual void handleAction(QAction * event);
 	QMenu * menu() {return _menu;}
@@ -188,17 +220,22 @@ private:
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;
     QAction * _aShowGrid;
+    QAction * _aSetGridCellCount;
+    QAction * _aSetGridCellSize;
     QAction * _aSetBackgroundColor;
     QMenu * _menu;
-    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr > _graphes;
+    std::set<std::string> _graphes;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
+    unsigned int _gridCellCount;
+    float _gridCellSize;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;
     QSet<Qt::Key> _keysPressed;
     QString _workingDirectory;
-    QColor _backgroundColor;
+    QColor _defaultBgColor;
+    QColor _currentBgColor;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DataRecorder.h

@@ -31,9 +31,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include <rtabmap/utilite/UEventsHandler.h>
-#include <QtGui/QWidget>
+#include <QWidget>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMutex.h>
+
+class QLabel;
 
 namespace rtabmap {
 
@@ -47,21 +50,29 @@ public:
 	DataRecorder(QWidget * parent = 0);
 	bool init(const QString & path, bool recordInRAM = true);
 
-	void close();
+	void closeRecorder();
 
 	virtual ~DataRecorder();
 
+	const QString & path() const {return path_;}
+
 public slots:
 	void addData(const rtabmap::SensorData & data);
-	void showImage(const rtabmap::SensorData & data);
+	void showImage(const cv::Mat & image, const cv::Mat & depth);
 protected:
+	virtual void closeEvent(QCloseEvent* event);
 	void handleEvent(UEvent * event);
 
 private:
+	UMutex memoryMutex_;
 	Memory * memory_;
 	ImageView* imageView_;
+	QLabel* label_;
 	UTimer timer_;
-	int dataQueue_;
+	QString path_;
+	bool processingImages_;
+	int count_;
+	int totalSizeKB_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include <QtGui/QMainWindow>
+#include <QMainWindow>
 #include <QtCore/QByteArray>
 #include <QtCore/QMap>
 #include <QtCore/QSet>
@@ -53,6 +53,7 @@ namespace rtabmap
 class Memory;
 class ImageView;
 class Signature;
+class CloudViewer;
 
 class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
 {
@@ -62,13 +63,19 @@ public:
 	DatabaseViewer(QWidget * parent = 0);
 	virtual ~DatabaseViewer();
 	bool openDatabase(const QString & path);
+	bool isSavedMaximized() const {return savedMaximized_;}
+	void showCloseButton(bool visible = true);
 
 protected:
 	virtual void showEvent(QShowEvent* anEvent);
+	virtual void moveEvent(QMoveEvent* anEvent);
 	virtual void resizeEvent(QResizeEvent* anEvent);
 	virtual void closeEvent(QCloseEvent* event);
+	virtual bool eventFilter(QObject *obj, QEvent *event);
 
 private slots:
+	void writeSettings();
+	void configModified();
 	void openDatabase();
 	void generateGraph();
 	void exportDatabase();
@@ -82,6 +89,7 @@ private slots:
 	void refineAllLoopClosureLinks();
 	void refineVisuallyAllNeighborLinks();
 	void refineVisuallyAllLoopClosureLinks();
+	void resetAllChanges();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);
@@ -89,6 +97,7 @@ private slots:
 	void sliderNeighborValueChanged(int);
 	void sliderLoopValueChanged(int);
 	void sliderIterationsValueChanged(int);
+	void updateGrid();
 	void updateGraphView();
 	void refineConstraint();
 	void refineConstraintVisually();
@@ -98,20 +107,31 @@ private slots:
 	void updateConstraintView();
 
 private:
+	QString getIniFilePath() const;
+	void readSettings();
+
 	void updateIds();
 	void update(int value,
 				QLabel * labelIndex,
 				QLabel * labelParents,
 				QLabel * labelChildren,
+				QLabel * weight,
+				QLabel * label,
+				QLabel * stamp,
 				rtabmap::ImageView * view,
+				rtabmap::CloudViewer * view3D,
 				QLabel * labelId,
-				bool updateConstraintView = true);
+				QLabel * labelMapId,
+				bool updateConstraintView);
 	void updateStereo(const Signature * data);
 	void updateWordsMatching();
-	void updateConstraintView(const rtabmap::Link & link,
+	void updateConstraintView(
+			const rtabmap::Link & link,
+			bool updateImageSliders = true,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
-			bool updateImageSliders = true);
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
 	void updateConstraintButtons();
 	Link findActiveLink(int from, int to);
 	bool containsLink(
@@ -121,8 +141,8 @@ private:
 	std::multimap<int, rtabmap::Link> updateLinksWithModifications(
 			const std::multimap<int, rtabmap::Link> & edgeConstraints);
 	void updateLoopClosuresSlider(int from = 0, int to = 0);
-	void refineConstraint(int from, int to, bool updateGraph);
+	void refineConstraint(int from, int to,  bool silent, bool updateGraph);
-	void refineConstraintVisually(int from, int to, bool updateGraph);
+	void refineConstraintVisually(int from, int to,  bool silent, bool updateGraph);
 	bool addConstraint(int from, int to, bool silent, bool updateGraph);
 
 private:
@@ -139,7 +159,10 @@ private:
 	std::multimap<int, rtabmap::Link> linksRefined_;
 	std::multimap<int, rtabmap::Link> linksAdded_;
 	std::multimap<int, rtabmap::Link> linksRemoved_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
+	std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles>
+
+	bool savedMaximized_;
+	bool firstCall_;
 };
 
 }

guilib/include/rtabmap/gui/ImageView.h

@@ -30,9 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include <QtGui/QGraphicsView>
+#include <QGraphicsView>
 #include <QtCore/QRectF>
 #include <QtCore/QMultiMap>
+#include <QtCore/QSettings>
 #include <opencv2/features2d/features2d.hpp>
 #include <map>
 
@@ -43,7 +44,7 @@ namespace rtabmap {
 
 class KeypointItem;
 
-class RTABMAPGUI_EXP ImageView : public QGraphicsView {
+class RTABMAPGUI_EXP ImageView : public QWidget {
 
 	Q_OBJECT
 
@@ -51,44 +52,64 @@ public:
 	ImageView(QWidget * parent = 0);
 	virtual ~ImageView();
 
-	void resetZoom();
+	void saveSettings(QSettings & settings, const QString & group = "") const;
+	void loadSettings(QSettings & settings, const QString & group = "");
 
-	bool isImageShown();
+	bool isImageShown() const;
-	bool isImageDepthShown();
+	bool isImageDepthShown() const;
-	bool isFeaturesShown();
+	bool isFeaturesShown() const;
-	bool isLinesShown();
+	bool isLinesShown() const;
+	int getAlpha() const {return _alpha;}
+	bool isGraphicsViewMode() const;
+	bool isGraphicsViewScaled() const;
+	const QColor & getBackgroundColor() const;
+
+	float viewScale() const;
 
 	void setFeaturesShown(bool shown);
 	void setImageShown(bool shown);
 	void setImageDepthShown(bool shown);
 	void setLinesShown(bool shown);
+	void setGraphicsViewMode(bool on);
+	void setGraphicsViewScaled(bool scaled);
+	void setBackgroundColor(const QColor & color);
 
-	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color = QColor(255, 255, 0, 70));
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
-	void setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color = QColor(255, 255, 0, 70));
+	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
+	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
+	void addLine(float x1, float y1, float x2, float y2, QColor color);
 	void setImage(const QImage & image);
 	void setImageDepth(const QImage & image);
-	void setFeatureColor(int id, const QColor & color);
+	void setFeatureColor(int id, QColor color);
-	void setFeaturesColor(const QColor & color);
+	void setFeaturesColor(QColor color);
+	void setAlpha(int alpha);
+	void setSceneRect(const QRectF & rect);
 
 	const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
 
 	void clearLines();
 	void clear();
 
+	virtual QSize sizeHint() const;
+
+signals:
+	void configChanged();
+
 protected:
+	virtual void paintEvent(QPaintEvent *event);
+	virtual void resizeEvent(QResizeEvent* event);
 	virtual void contextMenuEvent(QContextMenuEvent * e);
-	virtual void wheelEvent(QWheelEvent * e);
 
 private slots:
-	void updateZoom();
+	void sceneRectChanged(const QRectF &rect);
 
 private:
 	void updateOpacity();
+	void computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const;
 
 private:
-	int _zoom;
-	int _minZoom;
 	QString _savedFileName;
+	int _alpha;
 
 	QMenu * _menu;
 	QAction * _showImage;
@@ -96,10 +117,17 @@ private:
 	QAction * _showFeatures;
 	QAction * _showLines;
 	QAction * _saveImage;
+	QAction * _setAlpha;
+	QAction * _graphicsViewMode;
+	QAction * _graphicsViewScaled;
 
+	QGraphicsView * _graphicsView;
 	QMultiMap<int, rtabmap::KeypointItem *> _features;
-	QGraphicsPixmapItem * _image;
+	QList<QGraphicsLineItem*> _lines;
-	QGraphicsPixmapItem * _imageDepth;
+	QGraphicsPixmapItem * _imageItem;
+	QGraphicsPixmapItem * _imageDepthItem;
+	QPixmap _image;
+	QPixmap _imageDepth;
 };
 
 }

guilib/include/rtabmap/gui/KeypointItem.h

@@ -30,17 +30,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include <QtGui/QGraphicsEllipseItem>
+#include <QGraphicsEllipseItem>
-#include <QtGui/QGraphicsTextItem>
+#include <QGraphicsTextItem>
 #include <QtGui/QPen>
 #include <QtGui/QBrush>
+#include <opencv2/features2d/features2d.hpp>
 
 namespace rtabmap {
 
 class RTABMAPGUI_EXP KeypointItem : public QGraphicsEllipseItem
 {
 public:
-	KeypointItem(qreal x, qreal y, int r, const QString & info, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
+	KeypointItem(int id, const cv::KeyPoint & kpt, float depth = 0, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
 	virtual ~KeypointItem();
 
 	void setColor(const QColor & color);
@@ -56,9 +57,11 @@ private:
 	void hideDescription();
 
 private:
-	QString _info;
+	int _id;
+	cv::KeyPoint _kpt;
 	QGraphicsRectItem * _placeHolder;
 	int _width;
+	float _depth;
 };
 
 }

guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -34,7 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Signature.h>
 #include <opencv2/opencv.hpp>
-#include <QtGui/QWidget>
+#include <QWidget>
 
 class Ui_loopClosureViewer;
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include "rtabmap/utilite/UEventsHandler.h"
-#include <QtGui/QMainWindow>
+#include <QMainWindow>
 #include <QtCore/QSet>
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/SensorData.h"
@@ -66,6 +66,7 @@ class DetailedProgressDialog;
 class TwistGridWidget;
 class ExportCloudsDialog;
 class PostProcessingDialog;
+class DataRecorder;
 
 class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler
 {
@@ -102,6 +103,10 @@ public:
 
 	QString getWorkingDirectory() const;
 	void setMonitoringState(bool pauseChecked = false); // in monitoring state, only some actions are enabled
+	bool isSavedMaximized() const {return _savedMaximized;}
+
+	bool isProcessingStatistics() const {return _processingStatistics;}
+	bool isProcessingOdometry() const {return _processingOdometry;}
 
 public slots:
 	void processStats(const rtabmap::Statistics & stat);
@@ -109,14 +114,19 @@ public slots:
 protected:
 	virtual void closeEvent(QCloseEvent* event);
 	virtual void handleEvent(UEvent* anEvent);
+	virtual void showEvent(QShowEvent* anEvent);
+	virtual void moveEvent(QMoveEvent* anEvent);
 	virtual void resizeEvent(QResizeEvent* anEvent);
+	virtual bool eventFilter(QObject *obj, QEvent *event);
 
 private slots:
 	void changeState(MainWindow::State state);
 	void beep();
+	void configGUIModified();
+	void saveConfigGUI();
 	void newDatabase();
 	void openDatabase();
-	void closeDatabase();
+	bool closeDatabase();
 	void editDatabase();
 	void startDetection();
 	void pauseDetection();
@@ -138,13 +148,15 @@ private slots:
 	void selectOpenniCv();
 	void selectOpenniCvAsus();
 	void selectOpenni2();
+	void selectFreenect2();
+	void selectStereoDC1394();
+	void selectStereoFlyCapture2();
 	void dumpTheMemory();
 	void dumpThePrediction();
+	void sendGoal();
 	void downloadAllClouds();
 	void downloadPoseGraph();
 	void clearTheCache();
-	void saveFigures();
-	void loadFigures();
 	void openPreferences();
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
@@ -154,6 +166,7 @@ private slots:
 	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 changeImgRateSetting();
 	void changeDetectionRateSetting();
 	void changeTimeLimitSetting();
@@ -176,6 +189,7 @@ private slots:
 	void resetOdometry();
 	void triggerNewMap();
 	void dataRecorder();
+	void dataRecorderDestroyed();
 	void updateNodeVisibility(int, bool);
 
 signals:
@@ -185,6 +199,7 @@ signals:
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);
 	void rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap & event);
+	void rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent & event);
 	void imgRateChanged(double);
 	void detectionRateChanged(double);
 	void timeLimitChanged(float);
@@ -200,9 +215,10 @@ private:
 	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);
-	void updateSelectSourceImageMenu(int type);
+	void updateSelectSourceMenu();
-	void updateSelectSourceDatabase(bool used);
+	void applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent);
-	void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src);
+	void saveFigures();
+	void loadFigures();
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
 			const std::map<int, Transform> & poses,
@@ -253,13 +269,19 @@ private:
 	AboutDialog * _aboutDialog;
 	ExportCloudsDialog * _exportDialog;
 	PostProcessingDialog * _postProcessingDialog;
+	DataRecorder * _dataRecorder;
 
 	QSet<int> _lastIds;
 	int _lastId;
 	bool _processingStatistics;
 	bool _odometryReceived;
+	QString _newDatabasePath;
+	QString _newDatabasePathOutput;
 	QString _openedDatabasePath;
-	bool _emptyNewDatabase;
+	bool _databaseUpdated;
+	bool _odomImageShow;
+	bool _odomImageDepthShow;
+	bool _savedMaximized;
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
@@ -267,10 +289,12 @@ private:
 	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
 	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> > _occupancyLocalMaps; // <ground, obstacles>
+	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
+	std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
-	bool _lastOdometryProcessed;
+	bool _processingOdometry;
+	double _lastOdomInfoUpdateTime;
 
 	QTimer * _oneSecondTimer;
 	QTime * _elapsedTime;
@@ -288,6 +312,8 @@ private:
 
 	QVector<int> _refIds;
 	QVector<int> _loopClosureIds;
+
+	bool _firstCall;
 };
 
 }

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -31,47 +31,53 @@ 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/gui/CloudViewer.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"
-#include "rtabmap/utilite/UTimer.h"
+
-#include "rtabmap/utilite/UMutex.h"
+class QSpinBox;
+class QDoubleSpinBox;
+class QLabel;
 
 namespace rtabmap {
 
-class RTABMAPGUI_EXP OdometryViewer : public CloudViewer, public UEventsHandler
+class ImageView;
+class CloudViewer;
+
+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);
-	virtual ~OdometryViewer() {}
+	virtual ~OdometryViewer();
 
 public slots:
 	virtual void clear();
 
 protected:
-	void handleAction(QAction * a);
 	virtual void handleEvent(UEvent * event);
 
 private slots:
-	void processData();
+	void processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
 
 private:
-	UMutex dataMutex_;
+	ImageView* imageView_;
-	std::list<rtabmap::SensorData> data_;
+	CloudViewer* cloudView_;
-	int dataQuality_;
+	bool processingData_;
+	bool odomImageShow_;
+	bool odomImageDepthShow_;
+
 	Transform lastOdomPose_;
-	UTimer timer_;
-	int maxClouds_;
-	float voxelSize_;
-	int decimation_;
 	int qualityWarningThr_;
 	int id_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
+	QList<std::string> addedClouds_;
-	QAction * _aSetVoxelSize;
+
-	QAction * _aSetDecimation;
+	QSpinBox * maxCloudsSpin_;
-	QAction * _aSetCloudHistorySize;
+	QDoubleSpinBox * voxelSpin_;
-	QAction * _aPause;
+	QSpinBox * decimationSpin_;
+	QLabel * timeLabel_;
+	int validDecimationValue_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
-#include <QtGui/QDialog>
+#include <QDialog>
 #include <QtCore/QModelIndex>
 #include <QtCore/QVector>
 #include <set>
@@ -57,10 +57,10 @@ class QDoubleSpinBox;
 
 namespace rtabmap {
 
-class OdometryThread;
-class CameraThread;
 class Signature;
 class LoopClosureViewer;
+class CameraRGBD;
+class CalibrationDialog;
 
 class RTABMAPGUI_EXP PreferencesDialog : public QDialog
 {
@@ -87,7 +87,10 @@ public:
 		kSrcFreenect,
 		kSrcOpenNI_CV,
 		kSrcOpenNI_CV_ASUS,
-		kSrcOpenNI2
+		kSrcOpenNI2,
+		kSrcFreenect2,
+		kSrcStereoDC1394,
+		kSrcStereoFlyCapture2
 	};
 
 public:
@@ -97,10 +100,14 @@ public:
 	virtual QString getIniFilePath() const;
 	void init();
 
-	void saveWindowGeometry(const QString & windowName, const QWidget * window);
+	// save stuff
-	void loadWindowGeometry(const QString & windowName, QWidget * window);
+	void saveSettings();
+	void saveWindowGeometry(const QWidget * window);
+	void loadWindowGeometry(QWidget * window);
 	void saveMainWindowState(const QMainWindow * mainWindow);
-	void loadMainWindowState(QMainWindow * mainWindow);
+	void loadMainWindowState(QMainWindow * mainWindow, bool & maximized);
+	void saveWidgetState(const QWidget * widget);
+	void loadWidgetState(QWidget * widget);
 
 	void saveCustomConfig(const QString & section, const QString & key, const QString & value);
 	QString loadCustomConfig(const QString & section, const QString & key);
@@ -114,11 +121,10 @@ public:
 	int getGeneralLoggerType() const;
 	bool getGeneralLoggerPrintTime() const;
 	bool isVerticalLayoutUsed() const;
-	bool isImageFlipped() const;
 	bool imageRejectedShown() const;
 	bool imageHighestHypShown() const;
 	bool beepOnPause() const;
-	int getKeypointsOpacity() const;
+	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
 
@@ -146,19 +152,19 @@ public:
 
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;
-	bool getGridMapFillEmptySpace() const;
 	bool isGridMapFrom3DCloud() const;
-	int getGridMapFillEmptyRadius() const;
+	bool isGridMapEroded() const;
 	double getGridMapOpacity() const;
 
 	QString getWorkingDirectory() const;
 
 	// source panel
 	double getGeneralInputRate() const;
+	bool isSourceMirroring() const;
 	bool isSourceImageUsed() const;
 	bool isSourceDatabaseUsed() const;
-	bool isSourceOpenniUsed() const;
+	bool isSourceRGBDUsed() const;
-	int getSourceImageType() const;
+	PreferencesDialog::Src getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
 	int getSourceHeight() const;
@@ -171,18 +177,20 @@ public:
 	int getSourceUsbDeviceId() const;		//UsbDevice group
 	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
-	float getSourceOpenniFx() const; // Openni group
+	CameraRGBD * createCameraRGBD(bool forCalibration = false); // return camera should be deleted if not null
-	float getSourceOpenniFy() const; // Openni group
-	float getSourceOpenniCx() const; // Openni group
-	float getSourceOpenniCy() const; // Openni group
 
 	int getIgnoredDCComponents() const;
 
@@ -197,6 +205,7 @@ public:
 	double getLoopThr() const;
 	double getVpThr() const;
 	int getOdomStrategy() const;
+	QString getCameraInfoDir() const; // "workinfDir/camera_info"
 
 	//
 	void setMonitoringState(bool monitoringState) {_monitoringState = monitoringState;}
@@ -208,12 +217,12 @@ signals:
 public slots:
 	void setInputRate(double value);
 	void setDetectionRate(double value);
-	void setHardThr(int 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 calibrate();
 
 private slots:
 	void closeDialog ( QAbstractButton * button );
@@ -239,13 +248,9 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
-	void showOpenNI2GroupBox(bool);
+	void updateRGBDCameraGroupBoxVisibility();
-	void cleanOdometryTest();
 	void testOdometry();
-	void cleanRGBDCameraTest();
 	void testRGBDCamera();
-	void calibrate();
-	void resetCalibration();
 
 protected:
 	virtual void showEvent ( QShowEvent * event );
@@ -261,9 +266,11 @@ protected:
 	virtual bool readCoreSettings(const QString & filePath = QString());
 
 	virtual void writeSettings(const QString & filePath = QString());
-	virtual void writeGuiSettings(const QString & filePath = QString());
+	virtual void writeGuiSettings(const QString & filePath = QString()) const;
-	virtual void writeCameraSettings(const QString & filePath = QString());
+	virtual void writeCameraSettings(const QString & filePath = QString()) const;
-	virtual void writeCoreSettings(const QString & filePath = QString());
+	virtual void writeCoreSettings(const QString & filePath = QString()) const;
+
+	virtual QString getTmpIniFilePath() const;
 
 private:
 	bool validateForm();
@@ -294,9 +301,8 @@ private:
 
 	QProgressDialog * _progressDialog;
 
-	//Odometry test
+	//calibration
-	CameraThread * _cameraThread;
+	CalibrationDialog * _calibrationDialog;
-	OdometryThread * _odomThread;
 
 	QVector<QCheckBox*> _3dRenderingShowClouds;
 	QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;

guilib/src/AboutDialog.cpp

@@ -27,6 +27,8 @@ 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/Graph.h"
 #include "ui_aboutDialog.h"
 #include <opencv2/core/version.hpp>
 #include <pcl/pcl_config.h>
@@ -51,6 +53,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_version->setText(version);
 	_ui->label_opencv_version->setText(cv_version);
 	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
+	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraFreenect2::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraStereoDC1394::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
+
+	_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
 }
 
 AboutDialog::~AboutDialog()

guilib/src/AboutDialog.h

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef ABOUTDIALOG_H_
 #define ABOUTDIALOG_H_
 
-#include <QtGui/QDialog>
+#include <QDialog>
 #include <QtCore/QUrl>
 
 class Ui_aboutDialog;

guilib/src/CMakeLists.txt

@@ -17,11 +17,13 @@ SET(headers_ui
 	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
 	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
 	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
+	../include/${PROJECT_PREFIX}/gui/CameraViewer.h
 	../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
 	./ExportDialog.h
 	./PostProcessingDialog.h
 	./ExportCloudsDialog.h
 	./MapVisibilityWidget.h
+	./GraphViewer.h
 )
 
 SET(uis
@@ -41,17 +43,21 @@ SET(qrc
     ./GuiLib.qrc
 )
 
-# generate rules for building source files from the resources
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
-QT4_ADD_RESOURCES(srcs_qrc ${qrc})
+    # generate rules for building source files from the resources
-
+    QT4_ADD_RESOURCES(srcs_qrc ${qrc})
-#Generate .h files from the .ui files
-QT4_WRAP_UI(moc_uis ${uis})
-
-#This will generate moc_* for Qt
-QT4_WRAP_CPP(moc_srcs ${headers_ui})
-### Qt Gui stuff  end###
 
+    #Generate .h files from the .ui files
+    QT4_WRAP_UI(moc_uis ${uis})
 
+    #This will generate moc_* for Qt
+    QT4_WRAP_CPP(moc_srcs ${headers_ui})
+    ### Qt Gui stuff  end###
+ELSE()
+    QT5_ADD_RESOURCES(srcs_qrc ${qrc})
+    QT5_WRAP_UI(moc_uis ${uis})
+    QT5_WRAP_CPP(moc_srcs ${headers_ui})
+ENDIF()
 
 
 SET(SRC_FILES 
@@ -70,6 +76,7 @@ SET(SRC_FILES
 	./OdometryViewer.cpp
 	./LoopClosureViewer.cpp
 	./DataRecorder.cpp
+	./CameraViewer.cpp
 	./CalibrationDialog.cpp
 	./ExportDialog.cpp
 	./PostProcessingDialog.cpp
@@ -91,7 +98,9 @@ SET(INCLUDE_DIRS
 	${PCL_INCLUDE_DIRS}
 )
 
-INCLUDE(${QT_USE_FILE})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
 
 SET(LIBRARIES
     ${QT_LIBRARIES} 
@@ -112,7 +121,11 @@ add_definitions(${PCL_DEFINITIONS})
 # create a library from the source files
 ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 # Linking with Qt libraries
+
 TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
+IF("${RTABMAP_QT_VERSION}" STREQUAL "5")
+    QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport)
+ENDIF()
 
 SET_TARGET_PROPERTIES(
      rtabmap_gui