diff --git a/CMakeLists.txt b/CMakeLists.txt index d2ced370..9d1f8d6d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 10) -SET(RTABMAP_PATCH_VERSION 4) +SET(RTABMAP_PATCH_VERSION 10) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -121,26 +121,97 @@ IF(APPLE) ENDIF(APPLE) ####### DEPENDENCIES ####### -FIND_PACKAGE(OpenCV REQUIRED) -FIND_PACKAGE(PCL 1.7 REQUIRED) -FIND_PACKAGE(VTK REQUIRED) -IF("${VTK_MAJOR_VERSION}" EQUAL 5) - FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 -ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5) -FIND_PACKAGE(ZLIB REQUIRED) -FIND_PACKAGE(Freenect) -FIND_PACKAGE(freenect2 QUIET) -FIND_PACKAGE(OpenNI2) -FIND_PACKAGE(DC1394) -FIND_PACKAGE(G2O) -FIND_PACKAGE(FlyCapture2) +option(WITH_QT "Include Qt support" ON) +option(WITH_FREENECT "Include Freenect support" ON) +option(WITH_FREENECT2 "Include Freenect2 support" ON) +option(WITH_OPENNI2 "Include OpenNI2 support" ON) +option(WITH_DC1394 "Include dc1394 support" ON) +option(WITH_G2O "Include g2o support" ON) +option(WITH_GTSAM "Include GTSAM support" ON) +option(WITH_CVSBA "Include cvsba support" ON) +option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) + +FIND_PACKAGE(OpenCV REQUIRED QUIET) +FIND_PACKAGE(PCL 1.7 REQUIRED QUIET) +FIND_PACKAGE(ZLIB REQUIRED QUIET) +IF(OpenCV_FOUND) + MESSAGE(STATUS "Found OpenCV: ${OpenCV_INCLUDE_DIRS}") +ENDIF(OpenCV_FOUND) +IF(PCL_FOUND) + MESSAGE(STATUS "Found PCL: ${PCL_INCLUDE_DIRS}") +ENDIF(PCL_FOUND) +IF(ZLIB_FOUND) + MESSAGE(STATUS "Found ZLIB: ${ZLIB_INCLUDE_DIRS}") +ENDIF(ZLIB_FOUND) + +IF(WITH_QT) # If Qt is here, the GUI will be built -IF("${RTABMAP_QT_VERSION}" STREQUAL "4") - FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) -ELSE() - FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) -ENDIF() + IF("${RTABMAP_QT_VERSION}" STREQUAL "4") + FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) + ELSE() + FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg) + ENDIF() + IF(QT4_FOUND OR Qt5_FOUND) + FIND_PACKAGE(VTK REQUIRED) + IF("${VTK_MAJOR_VERSION}" EQUAL 5) + FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5 + ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5) + ENDIF(QT4_FOUND OR Qt5_FOUND) +ENDIF(WITH_QT) + +IF(WITH_FREENECT) + FIND_PACKAGE(Freenect QUIET) + IF(Freenect_FOUND) + MESSAGE(STATUS "Found Freenect: ${Freenect_INCLUDE_DIRS}") + ENDIF(Freenect_FOUND) +ENDIF(WITH_FREENECT) + +IF(WITH_FREENECT2) + FIND_PACKAGE(freenect2 QUIET) + IF(freenect2_FOUND) + MESSAGE(STATUS "Found freenect2: ${freenect2_INCLUDE_DIRS}") + ENDIF(freenect2_FOUND) +ENDIF(WITH_FREENECT2) + +IF(WITH_OPENNI2) + FIND_PACKAGE(OpenNI2 QUIET) + IF(OpenNI2_FOUND) + MESSAGE(STATUS "Found OpenNI2: ${OpenNI2_INCLUDE_DIRS}") + ENDIF(OpenNI2_FOUND) +ENDIF(WITH_OPENNI2) + +IF(WITH_DC1394) + FIND_PACKAGE(DC1394 QUIET) + IF(DC1394_FOUND) + MESSAGE(STATUS "Found DC1394: ${DC1394_INCLUDE_DIRS}") + ENDIF(DC1394_FOUND) +ENDIF(WITH_DC1394) + +IF(WITH_G2O) + FIND_PACKAGE(G2O QUIET) + IF(G2O_FOUND) + MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}") + ENDIF(G2O_FOUND) +ENDIF(WITH_G2O) + +IF(WITH_GTSAM) + FIND_PACKAGE(GTSAM QUIET) +ENDIF(WITH_GTSAM) + +IF(WITH_FLYCAPTURE2) + FIND_PACKAGE(FlyCapture2 QUIET) + IF(FlyCapture2_FOUND) + MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}") + ENDIF(FlyCapture2_FOUND) +ENDIF(WITH_FLYCAPTURE2) + +IF(WITH_CVSBA) + FIND_PACKAGE(cvsba) + IF(cvsba_FOUND) + MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}") + ENDIF(cvsba_FOUND) +ENDIF(WITH_CVSBA) ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) @@ -183,8 +254,8 @@ ADD_SUBDIRECTORY( corelib ) IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) ADD_SUBDIRECTORY( guilib ) ADD_SUBDIRECTORY( app ) -ELSE() - MESSAGE(STATUS "[WARNING] Qt not found, the GUI lib and the stand-alone application will not be compiled...") +ELSEIF(WITH_QT) + MESSAGE(WARNING "Qt not found, the GUI lib and the stand-alone application will not be compiled...") ENDIF() ADD_SUBDIRECTORY( tools ) @@ -213,6 +284,7 @@ set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include" "${PROJECT_SOURCE_DIR}/guilib/include" "${PROJECT_SOURCE_DIR}/utilite/include") set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}") +set(CONF_WITH_GUI ${WITH_QT}) configure_file(RTABMapConfig.cmake.in "${PROJECT_BINARY_DIR}/RTABMapConfig.cmake" @ONLY) @@ -346,44 +418,74 @@ ENDIF(OpenCV_FOUND) IF(Freenect_FOUND) MESSAGE(STATUS " With Freenect = YES") +ELSEIF(NOT WITH_FREENECT) +MESSAGE(STATUS " With Freenect = NO (WITH_FREENECT=OFF)") ELSE() MESSAGE(STATUS " With Freenect = NO (libfreenect not found)") ENDIF() IF(OpenNI2_FOUND) MESSAGE(STATUS " With OpenNI2 = YES") +ELSEIF(NOT WITH_OPENNI2) +MESSAGE(STATUS " With OpenNI2 = NO (WITH_OPENNI2=OFF)") ELSE() MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)") ENDIF() IF(freenect2_FOUND) MESSAGE(STATUS " With Freenect2 = YES") +ELSEIF(NOT WITH_FREENECT2) +MESSAGE(STATUS " With Freenect2 = NO (WITH_FREENECT2=OFF)") ELSE() MESSAGE(STATUS " With Freenect2 = NO (libfreenect2 not found)") ENDIF() IF(DC1394_FOUND) MESSAGE(STATUS " With dc1394 = YES") +ELSEIF(NOT WITH_DC1394) +MESSAGE(STATUS " With dc1394 = NO (WITH_DC1394=OFF)") ELSE() MESSAGE(STATUS " With dc1394 = NO (dc1394 not found)") ENDIF() IF(FlyCapture2_FOUND) MESSAGE(STATUS " With FlyCapture2/Triclops = YES") +ELSEIF(NOT WITH_FLYCAPTURE2) +MESSAGE(STATUS " With FlyCapture2/Triclops = NO (WITH_FLYCAPTURE2=OFF)") ELSE() MESSAGE(STATUS " With FlyCapture2/Triclops = NO (Point Grey SDK not found)") ENDIF() IF(G2O_FOUND) MESSAGE(STATUS " With g2o = YES") +ELSEIF(NOT WITH_G2O) +MESSAGE(STATUS " With g2o = NO (WITH_G2O=OFF)") ELSE() MESSAGE(STATUS " With g2o = NO (g2o not found)") ENDIF() +IF(GTSAM_FOUND) +MESSAGE(STATUS " With GTSAM = YES") +ELSEIF(NOT WITH_GTSAM) +MESSAGE(STATUS " With GTSAM = NO (WITH_GTSAM=OFF)") +ELSE() +MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)") +ENDIF() + +IF(cvsba_FOUND) +MESSAGE(STATUS " With cvsba = YES") +ELSEIF(NOT WITH_CVSBA) +MESSAGE(STATUS " With cvsba = NO (WITH_CVSBA=OFF)") +ELSE() +MESSAGE(STATUS " With cvsba = NO (cvsba not found)") +ENDIF() + IF(QT4_FOUND) MESSAGE(STATUS " With Qt = YES (version 4)") ELSEIF(Qt5_FOUND) MESSAGE(STATUS " With Qt = YES (version 5)") +ELSEIF(NOT WITH_QT) +MESSAGE(STATUS " With Qt = NO (WITH_QT=OFF)") ELSE() MESSAGE(STATUS " With Qt = NO (Qt not found, to use Qt5 you should set -DRTABMAP_QT_VERSION=5)") ENDIF() diff --git a/RTABMapConfig.cmake.in b/RTABMapConfig.cmake.in index 90a138f2..18284c90 100644 --- a/RTABMapConfig.cmake.in +++ b/RTABMapConfig.cmake.in @@ -2,12 +2,72 @@ # It defines the following variables # RTABMap_INCLUDE_DIRS - include directories for RTABMap # RTABMap_LIBRARIES - libraries to link against +# RTABMap_CORE - core library +# RTABMap_UTILITE - utilite library +# RTABMap_GUI - gui library (set if RTABMap is built with Qt) # Compute paths get_filename_component(RTABMap_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) set(RTABMap_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") -find_library(RTABMAP_CORE NAMES rtabmap_core rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMAP_GUI NAMES rtabmap_gui rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -find_library(RTABMAP_UTILITE NAMES rtabmap_utilite rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") -set(RTABMap_LIBRARIES ${RTABMAP_CORE} ${RTABMAP_GUI} ${RTABMAP_UTILITE}) \ No newline at end of file +#core lib +find_library(RTABMap_CORE_RELEASE NAMES rtabmap_core NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_CORE_DEBUG NAMES rtabmap_cored NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + +IF(RTABMap_CORE_DEBUG AND RTABMap_CORE_RELEASE) + SET(RTABMap_CORE + debug ${RTABMap_CORE_DEBUG} + optimized ${RTABMap_CORE_RELEASE} + ) +ELSEIF(RTABMap_CORE_RELEASE) + SET(RTABMap_CORE ${RTABMap_CORE_RELEASE}) +ELSEIF(RTABMap_CORE_DEBUG) + SET(RTABMap_CORE ${RTABMap_CORE_DEBUG}) +ENDIF() + +#utilite lib +find_library(RTABMap_UTILITE_RELEASE NAMES rtabmap_utilite NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") +find_library(RTABMap_UTILITE_DEBUG NAMES rtabmap_utilited NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + +IF(RTABMap_UTILITE_DEBUG AND RTABMap_UTILITE_RELEASE) + SET(RTABMap_UTILITE + debug ${RTABMap_UTILITE_DEBUG} + optimized ${RTABMap_UTILITE_RELEASE} + ) +ELSEIF(RTABMap_UTILITE_RELEASE) + SET(RTABMap_UTILITE ${RTABMap_UTILITE_RELEASE}) +ELSEIF(RTABMap_UTILITE_DEBUG) + SET(RTABMap_UTILITE ${RTABMap_UTILITE_DEBUG}) +ENDIF() + +set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE}) + +#gui lib (OFF if RTAB-Map is not built with Qt) +if(@CONF_WITH_GUI@) + find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS "@CONF_LIB_DIR@") + + IF(RTABMap_GUI_DEBUG AND RTABMap_GUI_RELEASE) + SET(RTABMap_GUI + debug ${RTABMap_GUI_DEBUG} + optimized ${RTABMap_GUI_RELEASE} + ) + ELSEIF(RTABMap_GUI_RELEASE) + SET(RTABMap_GUI ${RTABMap_GUI_RELEASE}) + ELSEIF(RTABMap_GUI_DEBUG) + SET(RTABMap_GUI ${RTABMap_GUI_DEBUG}) + ENDIF() + + set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI}) +endif(@CONF_WITH_GUI@) + +#backward compatibilities +if(RTABMap_CORE) + set(RTABMAP_CORE ${RTABMap_CORE}) +endif(RTABMap_CORE) +if(RTABMap_UTILITE) + set(RTABMAP_UTILITE ${RTABMap_UTILITE}) +endif(RTABMap_UTILITE) +if(RTABMap_GUI) + set(RTABMAP_GUI ${RTABMap_GUI}) +endif(RTABMap_GUI) \ No newline at end of file diff --git a/cmake_modules/FindFLANN.cmake b/cmake_modules/FindFLANN.cmake new file mode 100644 index 00000000..83b356fe --- /dev/null +++ b/cmake_modules/FindFLANN.cmake @@ -0,0 +1,68 @@ +############################################################################### +# Find FLANN +# +# This sets the following variables: +# FLANN_FOUND - True if FLANN was found. +# FLANN_INCLUDE_DIRS - Directories containing the FLANN include files. +# FLANN_LIBRARIES - Libraries needed to use FLANN. +# FLANN_DEFINITIONS - Compiler flags for FLANN. +# If FLANN_USE_STATIC is specified and then look for static libraries ONLY else +# look for shared ones +# +# Original from https://github.com/PointCloudLibrary/pcl/blob/master/cmake/Modules/FindFLANN.cmake +# + +if(FLANN_USE_STATIC) + set(FLANN_RELEASE_NAME flann_cpp_s) + set(FLANN_DEBUG_NAME flann_cpp_s-gd) +else(FLANN_USE_STATIC) + set(FLANN_RELEASE_NAME flann_cpp) + set(FLANN_DEBUG_NAME flann_cpp-gd) +endif(FLANN_USE_STATIC) + +find_package(PkgConfig QUIET) +if (FLANN_FIND_VERSION) + pkg_check_modules(PC_FLANN flann>=${FLANN_FIND_VERSION}) +else(FLANN_FIND_VERSION) + pkg_check_modules(PC_FLANN flann) +endif(FLANN_FIND_VERSION) + +set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER}) + +find_path(FLANN_INCLUDE_DIR flann/flann.hpp + HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES include) + +find_library(FLANN_LIBRARY + NAMES ${FLANN_RELEASE_NAME} + HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES lib) + +find_library(FLANN_LIBRARY_DEBUG + NAMES ${FLANN_DEBUG_NAME} ${FLANN_RELEASE_NAME} + HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS} "${FLANN_ROOT}" "$ENV{FLANN_ROOT}" + PATHS "$ENV{PROGRAMFILES}/Flann" "$ENV{PROGRAMW6432}/Flann" + PATH_SUFFIXES lib) + +if(NOT FLANN_LIBRARY_DEBUG) + set(FLANN_LIBRARY_DEBUG ${FLANN_LIBRARY}) +endif(NOT FLANN_LIBRARY_DEBUG) + +set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR}) +set(FLANN_LIBRARIES optimized ${FLANN_LIBRARY} debug ${FLANN_LIBRARY_DEBUG}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(FLANN DEFAULT_MSG FLANN_LIBRARY FLANN_INCLUDE_DIR) + +mark_as_advanced(FLANN_LIBRARY FLANN_LIBRARY_DEBUG FLANN_INCLUDE_DIR) + +if(FLANN_FOUND) + IF (NOT FLANN_FIND_QUIETLY) + message(STATUS "FLANN found (include: ${FLANN_INCLUDE_DIRS}, lib: ${FLANN_LIBRARIES})") + ENDIF (NOT FLANN_FIND_QUIETLY) + if(FLANN_USE_STATIC) + add_definitions(-DFLANN_STATIC) + endif(FLANN_USE_STATIC) +endif(FLANN_FOUND) diff --git a/cmake_modules/FindFlyCapture2.cmake b/cmake_modules/FindFlyCapture2.cmake index 0bbe8430..d96bc156 100644 --- a/cmake_modules/FindFlyCapture2.cmake +++ b/cmake_modules/FindFlyCapture2.cmake @@ -34,9 +34,6 @@ IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY}) ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY) -MESSAGE(STATUS "FlyCapture2_INCLUDE_DIRS={FlyCapture2_INCLUDE_DIRS}") -MESSAGE(STATUS "FlyCapture2_LIBRARIES={FlyCapture2_LIBRARIES}") - IF (FlyCapture2_FOUND) # show which FlyCapture2 was found only if not quiet IF (NOT FlyCapture2_FIND_QUIETLY) diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index 343ed530..892c0933 100644 --- a/cmake_modules/FindG2O.cmake +++ b/cmake_modules/FindG2O.cmake @@ -70,11 +70,13 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR}) SET(G2O_LIBRARIES ${G2O_STUFF_LIBRARY} - ${G2O_CORE_LIBRARY} + ${G2O_CORE_LIBRARY} + ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SLAM2D} ${G2O_TYPES_SLAM3D} - ${CSPARSE_LIBRARY}) + ${CSPARSE_LIBRARY} + cholmod) SET(G2O_FOUND "YES") ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND) diff --git a/corelib/src/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h similarity index 100% rename from corelib/src/BayesFilter.h rename to corelib/include/rtabmap/core/BayesFilter.h diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index 0db92a2f..3b22a6a7 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -96,15 +96,19 @@ public: void setLocalTransform(const Transform & transform) {localTransform_ = transform;} const Transform & localTransform() const {return localTransform_;} + void setImageSize(const cv::Size & size) {imageSize_ = size;} const cv::Size & imageSize() const {return imageSize_;} int imageWidth() const {return imageSize_.width;} - int imageWeight() const {return imageSize_.height;} + int imageHeight() const {return imageSize_.height;} bool load(const std::string & directory, const std::string & cameraName); bool save(const std::string & directory) const; void scale(double scale); + double horizontalFOV() const; // in degrees + double verticalFOV() const; // in degrees + // For depth images, your should use cv::INTER_NEAREST cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const; cv::Mat rectifyDepth(const cv::Mat & raw) const; diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index adbac999..548c2e4a 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -160,6 +160,7 @@ public: bool setExposure(int value); bool setGain(int value); bool setMirroring(bool enabled); + void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;} protected: virtual SensorData captureImage(); @@ -171,6 +172,7 @@ private: float _depthFx; float _depthFy; std::string _deviceId; + bool _openNI2StampsAndIDsUsed; }; diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 2662606a..752f0087 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -52,6 +52,7 @@ public: void setMirroringEnabled(bool enabled) {_mirroring = enabled;} void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} + void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} //getters bool isPaused() const {return !this->isRunning();} @@ -68,6 +69,7 @@ private: Camera * _camera; bool _mirroring; bool _colorOnly; + bool _stereoToDepth; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 721ef86c..b087dd30 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -60,6 +60,9 @@ class VisualWord; // class RTABMAP_EXP DBDriver : public UThreadNode { +public: + static DBDriver * create(const ParametersMap & parameters = ParametersMap()); + public: virtual ~DBDriver(); @@ -75,6 +78,15 @@ public: double getEmptyTrashesTime() const {return _emptyTrashesTime;} void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries + // Warning: the following functions don't look in the trash, direct database modifications + void generateGraph( + const std::string & fileName, + const std::set & ids = std::set(), + const std::map & otherSignatures = std::map()); + void addLink(const Link & link); + void removeLink(int from, int to); + void updateLink(const Link & link); + public: void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const; @@ -128,6 +140,8 @@ private: virtual void updateQuery(const std::list & signatures, bool updateTimestamp) const = 0; virtual void updateQuery(const std::list & words, bool updateTimestamp) const = 0; + virtual void addLinkQuery(const Link & link) const = 0; + virtual void updateLinkQuery(const Link & link) const = 0; // Load objects virtual void loadQuery(VWDictionary * dictionary) const = 0; diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index 1be224a0..91dd2c40 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -50,11 +50,13 @@ public: DBReader(const std::string & databasePath, float frameRate = 0.0f, bool odometryIgnored = false, - bool ignoreGoalDelay = false); + bool ignoreGoalDelay = false, + bool goalsIgnored = false); DBReader(const std::list & databasePaths, float frameRate = 0.0f, bool odometryIgnored = false, - bool ignoreGoalDelay = false); + bool ignoreGoalDelay = false, + bool goalsIgnored = false); virtual ~DBReader(); bool init(int startIndex=0); @@ -70,6 +72,7 @@ private: float _frameRate; // -1 = use Database stamps, 0 = inf bool _odometryIgnored; bool _ignoreGoalDelay; + bool _goalsIgnored; DBDriver * _dbDriver; UTimer _timer; diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index dda25071..940360c9 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { class Memory; @@ -49,7 +50,9 @@ public: enum Type { kTypeUndef = -1, kTypeTORO = 0, - kTypeG2O = 1 + kTypeG2O = 1, + kTypeGTSAM = 2, + kTypeCVSBA = 3 }; static Optimizer * create(const ParametersMap & parameters); static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap()); @@ -58,7 +61,7 @@ public: static void getConnectedGraph( int fromId, const std::map & posesIn, - const std::multimap & linksIn, + const std::multimap & linksIn, // only one link between two poses std::map & posesOut, std::multimap & linksOut, int depth = 0); @@ -72,12 +75,19 @@ public: bool isSlam2d() const {return slam2d_;} bool isCovarianceIgnored() const {return covarianceIgnored_;} double epsilon() const {return epsilon_;} + bool isRobust() const {return robust_;} + // inherited classes should implement one of these methods virtual std::map optimize( int rootId, const std::map & poses, const std::multimap & constraints, - std::list > * intermediateGraphes = 0) = 0; + std::list > * intermediateGraphes = 0); + virtual std::map optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures); virtual void parseParameters(const ParametersMap & parameters); @@ -86,7 +96,8 @@ protected: int iterations = Parameters::defaultRGBDOptimizeIterations(), bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), - double epsilon = Parameters::defaultRGBDOptimizeEpsilon()); + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()); Optimizer(const ParametersMap & parameters); private: @@ -94,6 +105,7 @@ private: bool slam2d_; bool covarianceIgnored_; double epsilon_; + bool robust_; }; class RTABMAP_EXP TOROOptimizer : public Optimizer @@ -109,8 +121,12 @@ public: std::multimap & edgeConstraints); public: - TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + TOROOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {} TOROOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~TOROOptimizer() {} @@ -128,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer { public: static bool available(); + static bool saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints = false); public: - G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + G2OOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + G2OOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~G2OOptimizer() {} @@ -145,6 +172,72 @@ public: std::list > * intermediateGraphes = 0); }; +class RTABMAP_EXP GTSAMOptimizer : public Optimizer +{ +public: + static bool available(); + +public: + GTSAMOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + + GTSAMOptimizer(const ParametersMap & parameters) : + Optimizer(parameters) {} + virtual ~GTSAMOptimizer() {} + + virtual Type type() const {return kTypeGTSAM;} + + virtual std::map optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes = 0); +}; + +class RTABMAP_EXP CVSBAOptimizer : public Optimizer +{ +public: + static bool available(); + +public: + CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : + Optimizer(iterations, slam2d, covarianceIgnored), + inlierDistance_(0.02), + minInliers_(10){} + CVSBAOptimizer(const ParametersMap & parameters) : + Optimizer(parameters), + inlierDistance_(0.02), + minInliers_(10){} + virtual ~CVSBAOptimizer() {} + + virtual Type type() const {return kTypeCVSBA;} + + void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;} + void setMinInliers(int minInliers) {minInliers_ = minInliers;} + + virtual std::map optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures); + +private: + float inlierDistance_; + float minInliers_; +}; + +bool RTABMAP_EXP exportPoses( + const std::string & filePath, + int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o) + const std::map & poses, + const std::multimap & constraints, // required for formats 3 and 4 + const std::map & stamps); // required for format 1 + //////////////////////////////////////////// // Graph utilities //////////////////////////////////////////// @@ -221,7 +314,9 @@ std::list > RTABMAP_EXP computePath( int toId, const Memory * memory, bool lookInDatabase = true, - bool updateNewCosts = false); + bool updateNewCosts = false, + float linearVelocity = 0.0f, // m/sec + float angularVelocity = 0.0f); // rad/sec int RTABMAP_EXP findNearestNode( const std::map & nodes, @@ -249,6 +344,10 @@ float RTABMAP_EXP computePathLength( unsigned int fromIndex = 0, unsigned int toIndex = 0); +std::list > RTABMAP_EXP getPaths( + std::map poses, + const std::multimap & links); + } /* namespace graph */ diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 59816069..34934fd3 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/corelib/include/rtabmap/core/Link.h @@ -29,8 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define LINK_H_ #include -#include -#include #include namespace rtabmap { @@ -39,38 +37,20 @@ class Link { public: enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef}; - Link() : - from_(0), - to_(0), - type_(kUndef), - infMatrix_(cv::Mat::eye(6,6,CV_64FC1)) - { - } + Link(); Link(int from, int to, Type type, const Transform & transform, - const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1)) : - from_(from), - to_(to), - transform_(transform), - type_(type) - { - setInfMatrix(infMatrix); - } + const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), + const cv::Mat & userData = cv::Mat()); Link(int from, int to, Type type, const Transform & transform, double rotVariance, - double transVariance) : - from_(from), - to_(to), - transform_(transform), - type_(type) - { - setVariance(rotVariance, transVariance); - } + double transVariance, + const cv::Mat & userData = cv::Mat()); bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;} @@ -79,65 +59,25 @@ public: const Transform & transform() const {return transform_;} Type type() const {return type_;} const cv::Mat & infMatrix() const {return infMatrix_;} - double rotVariance() const - { - double min = uMin3(infMatrix_.at(3,3), infMatrix_.at(4,4), infMatrix_.at(5,5)); - UASSERT(min > 0.0); - return 1.0/min; - } - double transVariance() const - { - double min = uMin3(infMatrix_.at(0,0), infMatrix_.at(1,1), infMatrix_.at(2,2)); - UASSERT(min > 0.0); - return 1.0/min; - } + double rotVariance() const; + double transVariance() const; void setFrom(int from) {from_ = from;} void setTo(int to) {to_ = to;} void setTransform(const Transform & transform) {transform_ = transform;} void setType(Type type) {type_ = type;} - void setInfMatrix(const cv::Mat & infMatrix) { - UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1); - UASSERT_MSG(uIsFinite(infMatrix.at(0,0)) && infMatrix.at(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(infMatrix.at(1,1)) && infMatrix.at(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(infMatrix.at(2,2)) && infMatrix.at(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(infMatrix.at(3,3)) && infMatrix.at(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(infMatrix.at(4,4)) && infMatrix.at(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(infMatrix.at(5,5)) && infMatrix.at(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)"); - infMatrix_ = infMatrix; - } - void setVariance(double rotVariance, double transVariance) { - UASSERT(uIsFinite(rotVariance) && rotVariance>0); - UASSERT(uIsFinite(transVariance) && transVariance>0); - infMatrix_ = cv::Mat::eye(6,6,CV_64FC1); - infMatrix_.at(0,0) = 1.0/transVariance; - infMatrix_.at(1,1) = 1.0/transVariance; - infMatrix_.at(2,2) = 1.0/transVariance; - infMatrix_.at(3,3) = 1.0/rotVariance; - infMatrix_.at(4,4) = 1.0/rotVariance; - infMatrix_.at(5,5) = 1.0/rotVariance; - } + void setInfMatrix(const cv::Mat & infMatrix); + void setVariance(double rotVariance, double transVariance); - Link merge(const Link & link) const - { - UASSERT(to_ == link.from()); - UASSERT(type_ == link.type()); - UASSERT(!transform_.isNull()); - UASSERT(!link.transform().isNull()); - UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); - UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1); - return Link( - from_, - link.to(), - type_, - transform_ * link.transform(), - infMatrix_ + link.infMatrix()); - } + void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw + void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too. + const cv::Mat & userDataRaw() const {return _userDataRaw;} + const cv::Mat & userDataCompressed() const {return _userDataCompressed;} + void uncompressUserData(); + cv::Mat uncompressUserDataConst() const; - Link inverse() const - { - return Link(to_, from_, type_, transform_.inverse(), infMatrix_); - } + Link merge(const Link & link, Type outputType) const; + Link inverse() const; private: int from_; @@ -145,6 +85,10 @@ private: Transform transform_; Type type_; cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1 + + // user data + cv::Mat _userDataCompressed; // compressed data + cv::Mat _userDataRaw; }; } diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 3015d458..f0c41c8c 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -48,7 +48,6 @@ namespace rtabmap { class Signature; class DBDriver; -class GraphNode; class VWDictionary; class VisualWord; class Feature2D; @@ -141,7 +140,7 @@ public: double & stamp, bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; - SensorData getNodeData(int nodeId, bool uncompressedData = false); + SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true); void getNodeWords(int nodeId, std::multimap & words, std::multimap & words3); @@ -164,10 +163,7 @@ public: virtual void dumpSignatures(const char * fileNameSign, bool words3D) const; void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const; - void generateGraph(const std::string & fileName, std::set ids = std::set()); - void createGraph(GraphNode * parent, - unsigned int maxDepth, - const std::set & endIds = std::set()); + void generateGraph(const std::string & fileName, const std::set & ids = std::set()); //keypoint stuff const VWDictionary * getVWDictionary() const; @@ -232,6 +228,7 @@ private: float _similarityThreshold; bool _rawDataKept; bool _binDataKept; + bool _saveDepth16Format; bool _notLinkedNodesKeptInDb; bool _incrementalMemory; int _maxStMemSize; @@ -280,6 +277,7 @@ private: int _bowEstimationType; double _bowPnPReprojError; int _bowPnPFlags; + bool _bowVarianceFromInliersCount; float _icpMaxTranslation; float _icpMaxRotation; int _icpDecimation; diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 091b3ff0..f202a7c0 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -88,6 +88,7 @@ private: int _estimationType; double _pnpReprojError; int _pnpFlags; + bool _varianceFromInliersCount; Transform _pose; int _resetCurrentCount; double previousStamp_; diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index ab775e77..7da1c8c0 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -43,6 +43,7 @@ public: features(-1), localMapSize(-1), timeEstimation(-1), + timeParticleFiltering(-1), stamp(0), interval(0), distanceTravelled(0), diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 1fc9cb0c..2e6e2acc 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -56,7 +56,7 @@ typedef std::pair ParametersPair; * DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));} * }; * DummyVideoImageWidth dummyVideoImageWidth; - * @endcode + * @endcode */ #define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \ public: \ @@ -156,7 +156,7 @@ typedef std::pair ParametersPair; * std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640" * @endcode * @see getDefaultParameters() - * TODO Add a detailed example with simple classes + * TODO Add a detailed example with simple classes */ class RTABMAP_EXP Parameters { @@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity."); RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM."); RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db."); + RTABMAP_PARAM(Mem, SaveDepth16Format, bool, true, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters)."); RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes)."); RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size."); RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode."); @@ -200,12 +201,12 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, "If > 0.0, voxelize laser scans when creating a signature."); RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true, "If local space links are kept in WM."); - // KeypointMemory (Keypoint-based) RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, ""); + RTABMAP_PARAM(Kp, IncrementalFlann, bool, true, "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size)."); RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf)"); - RTABMAP_PARAM(Kp, WordsPerImage, int, 400, ""); + RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction)."); RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)."); RTABMAP_PARAM_COND(Kp, NndrRatio, float, RTABMAP_NONFREE, 0.8, 0.9, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)"); RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); @@ -248,7 +249,7 @@ 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, QualityLevel, double, 0.01, ""); + RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, ""); RTABMAP_PARAM(GFTT, MinDistance, double, 5, ""); RTABMAP_PARAM(GFTT, BlockSize, int, 3, ""); RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, ""); @@ -283,18 +284,22 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC."); // RGB-D SLAM - RTABMAP_PARAM(RGBD, Enabled, bool, true, ""); - RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required)."); - RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); - RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); - RTABMAP_PARAM(RGBD, 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, Enabled, bool, true, ""); + RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required)."); + RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); + RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); + RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); + RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph."); + RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); + RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails."); + RTABMAP_PARAM(RGBD, PlanLinearVelocity, float, 0.0, "Linear velocity (m/sec) used to compute path weights."); + RTABMAP_PARAM(RGBD, PlanAngularVelocity, float, 0.0, "Angular velocity (rad/sec) used to compute path weights."); RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\"."); - RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); - RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); - RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); + RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); + RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); + RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer."); + RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data."); // Local loop closure detection RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM."); @@ -308,24 +313,26 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations."); RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses."); RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); - RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeRobust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies)."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); RTABMAP_PARAM(Odom, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)"); - RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits."); - RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences."); + RTABMAP_PARAM(Odom, MaxFeatures, int, 1000, "0 no limits."); + RTABMAP_PARAM(Odom, InlierDistance, float, 0.1, "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach."); RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform."); RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words."); - RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); - RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit)."); + RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); + RTABMAP_PARAM(Odom, MaxDepth, float, 0, "Max depth of the words (0 means no limit)."); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset)."); RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom]."); RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf)."); + RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers."); RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error."); RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory."); @@ -364,34 +371,35 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad)."); RTABMAP_PARAM(LccBow, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); - RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform."); - RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences."); + RTABMAP_PARAM(LccBow, MinInliers, int, 10, "Minimum visual word correspondences to compute geometry transform."); + RTABMAP_PARAM(LccBow, InlierDistance, float, 0.1, "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach."); RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words."); RTABMAP_PARAM(LccBow, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw)."); RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure."); RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error."); RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); + RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers."); RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure."); RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4."); RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio."); RTABMAP_PARAM(LccReextract, FeatureType, int, 4, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); - RTABMAP_PARAM(LccReextract, MaxWords, int, 600, "0 no limits."); + RTABMAP_PARAM(LccReextract, MaxWords, int, 1000, "0 no limits."); RTABMAP_PARAM(LccReextract, MaxDepth, float, 0.0, "Max depth of the words (0 means no limit)."); - RTABMAP_PARAM(LccIcp3, Decimation, int, 8, "Depth image decimation."); - RTABMAP_PARAM(LccIcp3, MaxDepth, float, 4.0, "Max cloud depth."); - RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.01, "Voxel size to be used for ICP computation."); + RTABMAP_PARAM(LccIcp3, Decimation, int, 4, "Depth image decimation."); + RTABMAP_PARAM(LccIcp3, MaxDepth, float, 3.0, "Max cloud depth."); + RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation."); RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set."); RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences."); RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations."); - RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.0, "Ratio of matching correspondences to accept the transform."); + RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.2, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane."); RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences."); RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations."); - RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.0, "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.025, "Voxel size to be used for ICP computation."); // Stereo disparity diff --git a/corelib/src/ParticleFilter.h b/corelib/include/rtabmap/core/ParticleFilter.h similarity index 100% rename from corelib/src/ParticleFilter.h rename to corelib/include/rtabmap/core/ParticleFilter.h diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index a80b05bf..dfbb3964 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -112,7 +112,7 @@ public: const std::string & path, bool optimized, bool global, - int type // 0=raw/KITTI format, 1=rgbd-slam format, 2=TORO + int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o ); void resetMemory(); void dumpPrediction() const; @@ -129,14 +129,18 @@ public: std::multimap & constraints, bool optimized, bool global, - std::map * signatures = 0); - void clearPath(); + std::map * signatures = 0); + + int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success + void clearPath(int status); // -1=failed 0=idle/executing 1=success bool computePath(int targetNode, bool global); bool computePath(const Transform & targetPose); // only in current optimized map const std::vector > & getPath() const {return _path;} std::vector > getPathNextPoses() const; std::vector getPathNextNodes() const; int getPathCurrentGoalId() const; + unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;} + unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;} const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;} std::map getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const; @@ -184,6 +188,7 @@ private: bool _poseScanMatching; bool _localLoopClosureDetectionTime; bool _localLoopClosureDetectionSpace; + bool _scanMatchingIdsSavedInLinks; float _localRadius; float _localImmunizationRatio; int _localDetectMaxGraphDepth; @@ -191,6 +196,7 @@ private: bool _localPathOdomPosesUsed; std::string _databasePath; bool _optimizeFromGraphEnd; + float _optimizationMaxLinearError; bool _reextractLoopClosureFeatures; int _reextractNNType; float _reextractNNDR; @@ -199,12 +205,16 @@ private: float _reextractMaxDepth; bool _startNewMapOnLoopClosure; float _goalReachedRadius; // meters - bool _planVirtualLinks; bool _goalsSavedInUserData; + int _pathStuckIterations; + float _pathLinearVelocity; + float _pathAngularVelocity; std::pair _loopClosureHypothesis; std::pair _highestHypothesis; double _lastProcessTime; + bool _someNodesHaveBeenTransferred; + float _distanceTravelled; // Abstract classes containing all loop closure // strategies for a type of signature or configuration. @@ -227,14 +237,17 @@ private: std::map _optimizedPoses; std::multimap _constraints; Transform _mapCorrection; - Transform _mapTransform; // for localization mode Transform _lastLocalizationPose; // for localization mode + int _lastLocalizationNodeId; // for localization mode // Planning stuff + int _pathStatus; std::vector > _path; + std::set _pathUnreachableNodes; unsigned int _pathCurrentIndex; unsigned int _pathGoalIndex; Transform _pathTransformToGoal; + int _pathStuckCount; }; diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index 295772cf..b5586f46 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -65,7 +65,7 @@ public: kCmdDumpMemory, kCmdDumpPrediction, kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin - kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=KITTI/raw format, 1=RGBD-SLAM format, 2=TORO) + kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o) kCmdCleanDataBuffer, kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly kCmdTriggerNewMap, @@ -202,13 +202,19 @@ public: RtabmapGlobalPathEvent(int goalId, const std::vector > & poses) : UEvent(goalId), _poses(poses) {} + RtabmapGlobalPathEvent(int goalId, const std::string & goalLabel, const std::vector > & poses) : + UEvent(goalId), + _goalLabel(goalLabel), + _poses(poses) {} virtual ~RtabmapGlobalPathEvent() {} int getGoal() const {return this->getCode();} + const std::string & getGoalLabel() const {return _goalLabel;} const std::vector > & getPoses() const {return _poses;} virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");} private: + std::string _goalLabel; std::vector > _poses; }; @@ -228,6 +234,16 @@ private: std::string _label; }; +class RtabmapGoalStatusEvent : public UEvent +{ +public: + RtabmapGoalStatusEvent(int status): + UEvent(status){} + + virtual ~RtabmapGoalStatusEvent() {} + virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");} +}; + } // namespace rtabmap #endif /* RTABMAPEVENT_H_ */ diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index e1fa028a..513a1cef 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -75,6 +75,7 @@ public: SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & cameraModel, @@ -95,6 +96,7 @@ public: SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & rgb, const cv::Mat & depth, const std::vector & cameraModels, @@ -115,6 +117,7 @@ public: SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & cameraModel, @@ -136,8 +139,8 @@ public: _laserScanCompressed.empty() && _cameraModels.size() == 0 && !_stereoCameraModel.isValid() && - !_userDataRaw.empty() && - !_userDataCompressed.empty() && + _userDataRaw.empty() && + _userDataCompressed.empty() && _keypoints.size() == 0 && _descriptors.empty()); } @@ -147,6 +150,7 @@ public: double stamp() const {return _stamp;} void setStamp(double stamp) {_stamp = stamp;} int laserScanMaxPts() const {return _laserScanMaxPts;} + float laserScanMaxRange() const {return _laserScanMaxRange;} const cv::Mat & imageCompressed() const {return _imageCompressed;} const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;} @@ -157,7 +161,7 @@ public: const cv::Mat & laserScanRaw() const {return _laserScanRaw;} void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;} void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;} - void setLaserScanRaw(const cv::Mat & laserScanRaw, int laserScanMaxPts) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = laserScanMaxPts;} + void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;} void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);} void setCameraModels(const std::vector & models) {_cameraModels = models;} void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;} @@ -190,6 +194,7 @@ private: int _id; double _stamp; int _laserScanMaxPts; + float _laserScanMaxRange; cv::Mat _imageCompressed; // compressed image cv::Mat _depthOrRightCompressed; // compressed image diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index cc2dcf56..61ee9462 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -57,11 +57,12 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, Highest_hypothesis_id,); RTABMAP_STATS(Loop, Highest_hypothesis_value,); RTABMAP_STATS(Loop, Vp_hypothesis,); - RTABMAP_STATS(Loop, ReactivateId,); + RTABMAP_STATS(Loop, Reactivate_id,); RTABMAP_STATS(Loop, Hypothesis_ratio,); RTABMAP_STATS(Loop, Hypothesis_reactivated,); - RTABMAP_STATS(Loop, VisualInliers,); + RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Last_id,); + RTABMAP_STATS(Loop, Optimization_max_error, m); RTABMAP_STATS(LocalLoop, Time_closures,); RTABMAP_STATS(LocalLoop, Space_last_closure_id,); @@ -83,8 +84,11 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Memory, Signatures_retrieved,); RTABMAP_STATS(Memory, Images_buffered,); RTABMAP_STATS(Memory, Rehearsal_sim,); + RTABMAP_STATS(Memory, Rehearsal_id,); RTABMAP_STATS(Memory, Rehearsal_merged,); RTABMAP_STATS(Memory, Local_graph_size,); + RTABMAP_STATS(Memory, Small_movement,); + RTABMAP_STATS(Memory, Distance_travelled, m); RTABMAP_STATS(Timing, Memory_update, ms); RTABMAP_STATS(Timing, Scan_matching, ms); @@ -118,6 +122,8 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(TimingMem, Compressing_data, ms); RTABMAP_STATS(Keypoint, Dictionary_size, words); + RTABMAP_STATS(Keypoint, Indexed_words, words); + RTABMAP_STATS(Keypoint, Index_memory_usage, KB); RTABMAP_STATS(Keypoint, Response_threshold,); public: diff --git a/corelib/include/rtabmap/core/Transform.h b/corelib/include/rtabmap/core/Transform.h index 2ed7ead8..682d45a5 100644 --- a/corelib/include/rtabmap/core/Transform.h +++ b/corelib/include/rtabmap/core/Transform.h @@ -51,6 +51,8 @@ public: Transform(const cv::Mat & transformationMatrix); // x,y,z, roll,pitch,yaw Transform(float x, float y, float z, float roll, float pitch, float yaw); + // x,y, theta + Transform(float x, float y, float theta); float r11() const {return data()[0];} float r12() const {return data()[1];} diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index cfa2303b..79bb4d85 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -41,6 +41,7 @@ namespace rtabmap class DBDriver; class VisualWord; +class FlannIndex; class RTABMAP_EXP VWDictionary { @@ -74,6 +75,8 @@ public: unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();} int getLastIndexedWordId() const; int getTotalActiveReferences() const {return _totalActiveReferences;} + unsigned int getIndexedWordsCount() const; + unsigned int getIndexMemoryUsed() const; void setNNStrategy(NNStrategy strategy); bool isIncremental() const {return _incrementalDictionary;} void setIncrementalDictionary(); @@ -97,14 +100,16 @@ protected: private: bool _incrementalDictionary; + bool _incrementalFlann; float _nndrRatio; std::string _dictionaryPath; // a pre-computed dictionary (.txt) bool _newWordsComparedTogether; int _lastWordId; - cv::flann::Index * _flannIndex; + FlannIndex * _flannIndex; cv::Mat _dataTree; NNStrategy _strategy; std::map _mapIndexId; + std::map _mapIdIndex; std::map _unusedWords; //, note that these words stay in _visualWords std::set _notIndexedWords; // Words that are not indexed in the dictionary std::set _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary diff --git a/corelib/src/VisualWord.h b/corelib/include/rtabmap/core/VisualWord.h similarity index 100% rename from corelib/src/VisualWord.h rename to corelib/include/rtabmap/core/VisualWord.h diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index 53576bc2..fd45266d 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/corelib/include/rtabmap/core/util2d.h @@ -41,7 +41,8 @@ namespace util2d cv::Mat RTABMAP_EXP disparityFromStereoImages( const cv::Mat & leftImage, - const cv::Mat & rightImage); + const cv::Mat & rightImage, + int type = CV_32FC1); // CV_32FC1 or CV_16SC1 cv::Mat RTABMAP_EXP disparityFromStereoImages( const cv::Mat & leftImage, @@ -53,6 +54,10 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages( double flowEps = 0.02, float maxCorrespondencesSlope = 0.1f); +cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, + float fx, float baseline, + int type = CV_32FC1); // CV_32FC1 or CV_16UC1 + cv::Mat RTABMAP_EXP depthFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, @@ -78,6 +83,9 @@ cv::Mat RTABMAP_EXP depthFromStereoCorrespondences( const std::vector & mask, float fx, float baseline); +cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); +cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); + float RTABMAP_EXP getDepth( const cv::Mat & depthImage, float x, float y, diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 4d841d94..10fda15a 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -127,9 +127,6 @@ pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( float maxDepth = 0, const Transform & localTransform = Transform::getIdentity()); -cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F); -cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U); - cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud); pcl::PointCloud::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan); @@ -147,10 +144,6 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D( const cv::Mat & disparity, float cx, float cy, float fx, float baseline); -cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, - float fx, float baseline, - int type = CV_32FC1); - pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( const std::list::Ptr> & clouds); pcl::PointCloud::Ptr RTABMAP_EXP concatenateClouds( diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index e9c2ac4e..c96ede46 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define UTIL3D_FILTERING_H_ #include +#include #include #include @@ -46,6 +47,9 @@ pcl::PointCloud::Ptr RTABMAP_EXP voxelize( pcl::PointCloud::Ptr RTABMAP_EXP voxelize( const pcl::PointCloud::Ptr & cloud, float voxelSize); +pcl::PointCloud::Ptr RTABMAP_EXP voxelize( + const pcl::PointCloud::Ptr & cloud, + float voxelSize); pcl::PointCloud::Ptr RTABMAP_EXP sampling( @@ -60,12 +64,31 @@ pcl::PointCloud::Ptr RTABMAP_EXP passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, - float max); + float max, + bool negative = false); pcl::PointCloud::Ptr RTABMAP_EXP passThrough( const pcl::PointCloud::Ptr & cloud, const std::string & axis, float min, - float max); + float max, + bool negative = false); + +pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( + const pcl::PointCloud::Ptr & cloud, + const Transform & cameraPose, + float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2 + float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2 + float nearClipPlaneDistance, + float farClipPlaneDistance, + bool negative = false); +pcl::PointCloud::Ptr RTABMAP_EXP frustumFiltering( + const pcl::PointCloud::Ptr & cloud, + const Transform & cameraPose, + float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2 + float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2 + float nearClipPlaneDistance, + float farClipPlaneDistance, + bool negative = false); pcl::PointCloud::Ptr RTABMAP_EXP removeNaNFromPointCloud( @@ -141,6 +164,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering( float radiusSearch, int minNeighborsInRadius = 0); +/** + * For convenience. + */ +pcl::PointCloud::Ptr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::PointCloud::Ptr & substractCloud, + float radiusSearch, + int minNeighborsInRadius = 0); + +/** + * Subtract a cloud from another one using radius filtering. + * @param cloud the input cloud. + * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked. + * @param cloud the input cloud to subtract. + * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked. + * @param radiusSearch the radius in meter. + * @return the indices of the points satisfying the parameters. + */ +pcl::IndicesPtr RTABMAP_EXP subtractFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + const pcl::PointCloud::Ptr & substractCloud, + const pcl::IndicesPtr & substractIndices, + float radiusSearch, + int minNeighborsInRadius = 0); + /** * For convenience. diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h index fceb7e7a..7f48ed06 100644 --- a/corelib/include/rtabmap/core/util3d_mapping.h +++ b/corelib/include/rtabmap/core/util3d_mapping.h @@ -47,7 +47,9 @@ void RTABMAP_EXP occupancy2DFromLaserScan( const cv::Mat & scan, cv::Mat & ground, cv::Mat & obstacles, - float cellSize); + float cellSize, + bool unknownSpaceFilled = false, + float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps( const std::map & poses, @@ -64,7 +66,8 @@ cv::Mat RTABMAP_EXP create2DMap(const std::map & poses, bool unknownSpaceFilled, float & xMin, float & yMin, - float minMapSize = 0.0f); + float minMapSize = 0.0f, + float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true void RTABMAP_EXP rayTrace(const cv::Point2i & start, const cv::Point2i & end, diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index d6ecfec2..b794beb1 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -30,10 +30,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - #include #include #include +#include +#include +#include namespace rtabmap { @@ -49,7 +51,14 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh( float gp3MaximumSurfaceAngle = M_PI/4, float gp3MinimumAngle = M_PI/18, float gp3MaximumAngle = 2*M_PI/3, - bool gp3NormalConsistency = false); + bool gp3NormalConsistency = true); + +pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh( + const pcl::PolygonMesh::Ptr & mesh, + const std::map & poses, + const std::map & cameraModels, + const std::map & images, + const std::string & tmpDirectory = "."); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, @@ -59,10 +68,54 @@ pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, int normalKSearch = 20); -pcl::PointCloud::Ptr RTABMAP_EXP computeNormalsSmoothed( +pcl::PointCloud::Ptr RTABMAP_EXP mls( const pcl::PointCloud::Ptr & cloud, - float smoothingSearchRadius = 0.025, - bool smoothingPolynomialFit = true); + float searchRadius = 0.0f, + int polygonialOrder = 2, + int upsamplingMethod = 0, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION + float upsamplingRadius = 0.0f, // SAMPLE_LOCAL_PLANE + float upsamplingStep = 0.0f, // SAMPLE_LOCAL_PLANE + int pointDensity = 0, // RANDOM_UNIFORM_DENSITY + float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION + int dilationIterations = 0); // VOXEL_GRID_DILATION + +void RTABMAP_EXP adjustNormalsToViewPoints( + const std::map & poses, + const pcl::PointCloud::Ptr & rawCloud, + const std::vector & rawCameraIndices, + pcl::PointCloud::Ptr & cloud); + +pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor); + +template +std::vector normalizePolygonsSide( + const pcl::PointCloud & cloud, + const std::vector & polygons, + const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0)) +{ + std::vector output(polygons.size()); + for(unsigned int i=0; i::Ptr RTABMAP_EXP transformPointCloud( pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( const pcl::PointCloud::Ptr & cloud, const Transform & transform); +pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform); +pcl::PointCloud::Ptr RTABMAP_EXP transformPointCloud( + const pcl::PointCloud::Ptr & cloud, + const Transform & transform); pcl::PointXYZ RTABMAP_EXP transformPoint( const pcl::PointXYZ & pt, diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index e80f142f..5d932b33 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "BayesFilter.h" +#include "rtabmap/core/BayesFilter.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/Parameters.h" @@ -289,9 +289,9 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector for(std::list::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) { float sum = 0.0f; // sum values added - sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap); + sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap); idsDone.insert(*iter); - this->normalize(prediction, i, sum, ids[0]<0); + this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0); } } else @@ -451,6 +451,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, UDEBUG("time getting removed ids = %fs", timer.restart()); int added = 0; + float epsilon = 0.00001f; // get ids to update std::set idsToUpdate; for(unsigned int i=0; i epsilon && j!=i && removedIds.find(oldIds[j]) == removedIds.end()) { //UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]); idsToUpdate.insert(oldIds[j]); + ++count; } } + UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count); } } if(iaddNeighborProb(prediction, i, neighbors, newIdToIndexMap); this->normalize(prediction, i, sum, newIds[0]<0); ++added; + int count = 0; for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) { if(uContains(oldIdToIndexMap, iter->first) && removedIds.find(iter->first) == removedIds.end()) { idsToUpdate.insert(iter->first); + ++count; } } + UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count); } } - UDEBUG("time getting ids to update = %fs", timer.restart()); + UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart()); // update modified/added ids int modified = 0; + std::set idsDone; for(std::set::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter) { - std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); - int index = newIdToIndexMap.at(*iter); - float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); - this->normalize(prediction, index, sum, newIds[0]<0); - ++modified; + if(idsDone.find(*iter) == idsDone.end() && *iter > 0) + { + std::map neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true); + + std::list idsLoopMargin; + //filter neighbors in STM + for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end();) + { + if(memory->isInSTM(jter->first)) + { + neighbors.erase(jter++); + } + else + { + if(jter->second == 0) + { + idsLoopMargin.push_back(jter->first); + } + ++jter; + } + } + + // should at least have 1 id in idsMarginLoop + if(idsLoopMargin.size() == 0) + { + UFATAL("No 0 margin neighbor for signature %d !?!?", *iter); + } + + // same neighbor tree for loop signatures (margin = 0) + for(std::list::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter) + { + int index = newIdToIndexMap.at(*iter); + float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap); + idsDone.insert(*iter); + this->normalize(prediction, index, sum, newIds[0]<0); + ++modified; + } + } } - UDEBUG("time updating modified/added ids = %fs", timer.restart()); + UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart()); //UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows); //UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows); @@ -510,15 +551,22 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, { if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end()) { - for(int j=0; j epsilon) { //UDEBUG("i=%d, j=%d", i, j); //UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]); //UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i])); //UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j])); - ((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols]; + float v = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols]; + int ii = newIdToIndexMap.at(oldIds[i]); + int jj = newIdToIndexMap.at(oldIds[j]); + ((float *)prediction.data)[ii + jj*prediction.cols] = v; + if(ii != jj) + { + ((float *)prediction.data)[jj + ii*prediction.cols] = v; + } } } ++copied; @@ -536,6 +584,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, for(int j=1; j0) diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 6af33c18..9afae0de 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -42,6 +42,7 @@ SET(SRC_FILES SensorData.cpp Graph.cpp Compression.cpp + Link.cpp Odometry.cpp OdometryThread.cpp @@ -57,6 +58,9 @@ SET(SRC_FILES toro3d/posegraph2.cpp toro3d/treeoptimizer2.cpp + rtflann/ext/lz4.c + rtflann/ext/lz4hc.c + sqlite3/sqlite3.c ) @@ -149,8 +153,44 @@ IF(G2O_FOUND) ${LIBRARIES} ${G2O_LIBRARIES} ) + #Newest versions require std11 + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + + SET(SRC_FILES + ${SRC_FILES} + vertigo/g2o/edge_se2MaxMixture.cpp + vertigo/g2o/edge_se2Switchable.cpp + vertigo/g2o/edge_se3Switchable.cpp + vertigo/g2o/edge_switchPrior.cpp + vertigo/g2o/types_g2o_robust.cpp + vertigo/g2o/vertex_switchLinear.cpp + ) ENDIF(G2O_FOUND) +IF(GTSAM_FOUND) + ADD_DEFINITIONS("-DWITH_GTSAM") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + gtsam + ) +ENDIF(GTSAM_FOUND) + +IF(cvsba_FOUND) + ADD_DEFINITIONS("-DWITH_CVSBA") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${cvsba_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${cvsba_LIBS} + ) +ENDIF(cvsba_FOUND) + #################################### # Generate resources files #################################### diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 13cd868b..46e79f07 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -231,6 +231,17 @@ bool CameraModel::save(const std::string & directory) const fs << "data" << std::vector((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols)); fs << "}"; + // compaibility with ROS + + if(D_.cols > 5) + { + fs << "distortion_model" << "rational_polynomial"; + } + else + { + fs << "distortion_model" << "plumb_bob"; + } + fs << "rectification_matrix" << "{"; fs << "rows" << R_.rows; fs << "cols" << R_.cols; @@ -266,6 +277,24 @@ void CameraModel::scale(double scale) P_.at(1,2) *= scale; } +double CameraModel::horizontalFOV() const +{ + if(imageWidth() > 0 && fx() > 0.0) + { + return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI; + } + return 0.0; +} + +double CameraModel::verticalFOV() const +{ + if(imageHeight() > 0 && fy() > 0.0) + { + return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI; + } + return 0.0; +} + cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const { if(!mapX_.empty() && !mapY_.empty()) diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index e6dc5aa6..2070bef1 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -390,7 +390,8 @@ CameraOpenNI2::CameraOpenNI2( #endif _depthFx(0.0f), _depthFy(0.0f), - _deviceId(deviceId) + _deviceId(deviceId), + _openNI2StampsAndIDsUsed(false) { } @@ -715,7 +716,14 @@ SensorData CameraOpenNI2::captureImage() float(rgb.cols/2) - 0.5f, //cx float(rgb.rows/2) - 0.5f, //cy this->getLocalTransform()); - data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + if(_openNI2StampsAndIDsUsed) + { + data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0); + } + else + { + data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); + } } } } diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 7610fb0d..bf4dcac8 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Camera.h" #include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/CameraRGBD.h" +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" #include #include @@ -40,7 +42,8 @@ namespace rtabmap CameraThread::CameraThread(Camera * camera) : _camera(camera), _mirroring(false), - _colorOnly(false) + _colorOnly(false), + _stereoToDepth(false) { UASSERT(_camera != 0); } @@ -96,6 +99,16 @@ void CameraThread::mainLoop() data.setDepthOrRightRaw(tmpDepth); } } + if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty()) + { + cv::Mat depth = util2d::depthFromDisparity( + util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()), + data.stereoCameraModel().left().fx(), + data.stereoCameraModel().baseline()); + data.setCameraModel(data.stereoCameraModel().left()); + data.setDepthOrRightRaw(depth); + data.setStereoCameraModel(StereoCameraModel()); + } this->post(new CameraEvent(data, _camera->getSerial())); } diff --git a/corelib/src/Compression.cpp b/corelib/src/Compression.cpp index 3d15a7ee..c549052f 100644 --- a/corelib/src/Compression.cpp +++ b/corelib/src/Compression.cpp @@ -88,7 +88,16 @@ std::vector compressImage(const cv::Mat & image, const std::strin std::vector bytes; if(!image.empty()) { - cv::imencode(format, image, bytes); + if(image.type() == CV_32FC1) + { + //save in 8bits-4channel + cv::Mat bgra(image.size(), CV_8UC4, image.data); + cv::imencode(format, bgra, bytes); + } + else + { + cv::imencode(format, image, bytes); + } } return bytes; } @@ -114,6 +123,10 @@ cv::Mat uncompressImage(const cv::Mat & bytes) #else image = cv::imdecode(bytes, -1); #endif + if(image.type() == CV_8UC4) + { + image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + } } return image; } @@ -128,6 +141,10 @@ cv::Mat uncompressImage(const std::vector & bytes) #else image = cv::imdecode(bytes, -1); #endif + if(image.type() == CV_8UC4) + { + image = cv::Mat(image.size(), CV_32FC1, image.data).clone(); + } } return image; } diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 6ec9ae91..0e99b46b 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -28,15 +28,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/DBDriver.h" #include "rtabmap/core/Signature.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UMath.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UStl.h" +#include "DBDriverSqlite3.h" namespace rtabmap { +DBDriver * DBDriver::create(const ParametersMap & parameters) +{ + // well, we only have Sqlite3 database type for now :P + return new DBDriverSqlite3(parameters); +} + DBDriver::DBDriver(const ParametersMap & parameters) : _emptyTrashesTime(0), _timestampUpdate(true) @@ -291,6 +298,23 @@ void DBDriver::saveOrUpdate(const std::vector & words) const } } +void DBDriver::addLink(const Link & link) +{ + _dbSafeAccessMutex.lock(); + this->addLinkQuery(link); + _dbSafeAccessMutex.unlock(); +} +void DBDriver::removeLink(int from, int to) +{ + this->executeNoResult(uFormat("DELETE FROM Link WHERE from_id=%d and to_id=%d", from, to).c_str()); +} +void DBDriver::updateLink(const Link & link) +{ + _dbSafeAccessMutex.lock(); + this->updateLinkQuery(link); + _dbSafeAccessMutex.unlock(); +} + void DBDriver::load(VWDictionary * dictionary) const { _dbSafeAccessMutex.lock(); @@ -715,4 +739,155 @@ void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce } } +void DBDriver::generateGraph( + const std::string & fileName, + const std::set & idsInput, + const std::map & otherSignatures) +{ + if(this->isConnected()) + { + if(!fileName.empty()) + { + FILE* fout = 0; + #ifdef _MSC_VER + fopen_s(&fout, fileName.c_str(), "w"); + #else + fout = fopen(fileName.c_str(), "w"); + #endif + + if (!fout) + { + UERROR("Cannot open file %s!", fileName.c_str()); + return; + } + + std::set ids; + if(idsInput.size() == 0) + { + this->getAllNodeIds(ids); + UDEBUG("ids.size()=%d", ids.size()); + for(std::map::const_iterator iter=otherSignatures.begin(); iter!=otherSignatures.end(); ++iter) + { + ids.insert(iter->first); + } + } + else + { + ids = idsInput; + } + + const char * colorG = "green"; + const char * colorP = "pink"; + ; UINFO("Generating map with %d locations", ids.size()); + fprintf(fout, "digraph G {\n"); + for(std::set::iterator i=ids.begin(); i!=ids.end(); ++i) + { + if(otherSignatures.find(*i) == otherSignatures.end()) + { + int id = *i; + std::map links; + this->loadLinks(id, links); + int weight = 0; + this->getWeight(id, weight); + for(std::map::iterator iter = links.begin(); iter!=links.end(); ++iter) + { + int weightNeighbor = 0; + if(otherSignatures.find(iter->first) == otherSignatures.end()) + { + this->getWeight(iter->first, weightNeighbor); + } + else + { + weightNeighbor = otherSignatures.find(iter->first)->second->getWeight(); + } + //UDEBUG("Add neighbor link from %d to %d", id, iter->first); + if(iter->second.type() == Link::kNeighbor) + { + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", + id, + weight, + iter->first, + weightNeighbor); + } + else if(iter->first > id) + { + //loop + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorG); + } + else + { + //child + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorP); + } + } + } + } + for(std::map::const_iterator i=otherSignatures.begin(); i!=otherSignatures.end(); ++i) + { + if(ids.find(i->first) != ids.end()) + { + int id = i->second->id(); + const std::map & links = i->second->getLinks(); + int weight = i->second->getWeight(); + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + { + int weightNeighbor = 0; + const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0); + if(s) + { + weightNeighbor = s->getWeight(); + } + else + { + this->getWeight(iter->first, weightNeighbor); + } + //UDEBUG("Add neighbor link from %d to %d", id, iter->first); + if(iter->second.type() == Link::kNeighbor) + { + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", + id, + weight, + iter->first, + weightNeighbor); + } + else if(iter->first > id) + { + //loop + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorG); + } + else + { + //child + fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", + id, + weight, + iter->first, + weightNeighbor, + colorP); + } + } + } + } + fprintf(fout, "}\n"); + fclose(fout); + UINFO("Graph saved to \"%s\" (Tip: $ neato -Tpdf \"%s\" -o out.pdf)", fileName.c_str(), fileName.c_str()); + } + } +} + } // namespace rtabmap diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 771ffdf1..978ee1ed 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "DBDriverSqlite3.h" #include "rtabmap/core/Signature.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/VWDictionary.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/Compression.h" @@ -457,7 +457,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) con sqlite3_stmt * ppStmt = 0; std::stringstream query; - if(uStrNumCmp(_version, "0.10.1") >= 0) + if(uStrNumCmp(_version, "0.10.7") >= 0) + { + query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data " + << "FROM Data " + << "WHERE id = ?" + <<";"; + } + else if(uStrNumCmp(_version, "0.10.1") >= 0) { query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data " << "FROM Data " @@ -653,6 +660,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) con laserScanMaxPts = sqlite3_column_int(ppStmt, index++); } + float laserScanMaxRange = 0.0f; + if(uStrNumCmp(_version, "0.10.7") >= 0) + { + laserScanMaxRange = sqlite3_column_int(ppStmt, index++); + } + data = sqlite3_column_blob(ppStmt, index); dataSize = sqlite3_column_bytes(ppStmt, index++); //Create the laserScan @@ -685,6 +698,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) con (*iter)->sensorData() = SensorData( scanCompressed, laserScanMaxPts, + laserScanMaxRange, imageCompressed, depthOrRightCompressed, models, @@ -697,6 +711,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) con (*iter)->sensorData() = SensorData( scanCompressed, laserScanMaxPts, + laserScanMaxRange, imageCompressed, depthOrRightCompressed, stereoModel, @@ -1537,7 +1552,11 @@ void DBDriverSqlite3::loadLinksQuery( sqlite3_stmt * ppStmt = 0; std::stringstream query; - if(uStrNumCmp(_version, "0.8.4") >= 0) + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + query << "SELECT to_id, type, transform, rot_variance, trans_variance, user_data FROM Link "; + } + else if(uStrNumCmp(_version, "0.8.4") >= 0) { query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link "; } @@ -1603,7 +1622,20 @@ void DBDriverSqlite3::loadLinksQuery( { rotVariance = sqlite3_column_double(ppStmt, index++); transVariance = sqlite3_column_double(ppStmt, index++); - neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance))); + + cv::Mat userDataCompressed; + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + const void * data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + //Create the userData + if(dataSize>4 && data) + { + userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData + } + } + + neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance, userDataCompressed))); } else if(uStrNumCmp(_version, "0.7.4") >= 0) { @@ -1643,7 +1675,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list & signatures) const std::stringstream query; int totalLinksLoaded = 0; - if(uStrNumCmp(_version, "0.8.4") >= 0) + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + query << "SELECT to_id, type, rot_variance, trans_variance, user_data, transform FROM Link " + << "WHERE from_id = ? " + << "ORDER BY to_id"; + } + else if(uStrNumCmp(_version, "0.8.4") >= 0) { query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link " << "WHERE from_id = ? " @@ -1687,10 +1725,22 @@ void DBDriverSqlite3::loadLinksQuery(std::list & signatures) const toId = sqlite3_column_int(ppStmt, index++); linkType = sqlite3_column_int(ppStmt, index++); + cv::Mat userDataCompressed; if(uStrNumCmp(_version, "0.8.4") >= 0) { rotVariance = sqlite3_column_double(ppStmt, index++); transVariance = sqlite3_column_double(ppStmt, index++); + + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + const void * data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + //Create the userData + if(dataSize>4 && data) + { + userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData + } + } } else if(uStrNumCmp(_version, "0.7.4") >= 0) { @@ -1714,11 +1764,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list & signatures) const { if(uStrNumCmp(_version, "0.7.4") >= 0) { - links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance)); + links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance, userDataCompressed)); } 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, rotVariance, transVariance)); + links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance, userDataCompressed)); } } else @@ -2129,6 +2179,61 @@ void DBDriverSqlite3::saveQuery(const std::list & words) const } } +void DBDriverSqlite3::addLinkQuery(const Link & link) const +{ + UDEBUG(""); + if(_ppDb) + { + std::string type; + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + + // Create new entries in table Link + std::string query = queryStepLink(); + 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()); + + // Save link + stepLink(ppStmt, link); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + + UDEBUG("Time=%fs", timer.ticks()); + } + +} + +void DBDriverSqlite3::updateLinkQuery(const Link & link) const +{ + UDEBUG(""); + if(_ppDb) + { + std::string type; + UTimer timer; + timer.start(); + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + + // Create new entries in table Link + std::string query = queryStepLinkUpdate(); + 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()); + + // Save link + stepLink(ppStmt, link); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + + UDEBUG("Time=%fs", timer.ticks()); + } +} + std::string DBDriverSqlite3::queryStepNode() const { if(uStrNumCmp(_version, "0.10.1") >= 0) @@ -2360,7 +2465,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor std::string DBDriverSqlite3::queryStepSensorData() const { UASSERT(uStrNumCmp(_version, "0.10.0") >= 0); - if(uStrNumCmp(_version, "0.10.1") >= 0) + if(uStrNumCmp(_version, "0.10.7") >= 0) + { + return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);"; + } + else if(uStrNumCmp(_version, "0.10.1") >= 0) { return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);"; } @@ -2455,6 +2564,13 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + // scan_max_range + if(uStrNumCmp(_version, "0.10.7") >= 0) + { + rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + } + // scan if(!sensorData.laserScanCompressed().empty()) { @@ -2488,19 +2604,43 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt, UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); } -std::string DBDriverSqlite3::queryStepLink() const +std::string DBDriverSqlite3::queryStepLinkUpdate() const { - if(uStrNumCmp(_version, "0.8.4") >= 0) + if(uStrNumCmp(_version, "0.10.10") >= 0) { - return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);"; + return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=?, user_data=? WHERE from_id=? AND to_id = ?;"; + } + else if(uStrNumCmp(_version, "0.8.4") >= 0) + { + return "UPDATE Link SET type=?, rot_variance=?, trans_variance=?, transform=? WHERE from_id=? AND to_id = ?;"; } else if(uStrNumCmp(_version, "0.7.4") >= 0) { - return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);"; + return "UPDATE Link SET type=?, variance=?, transform=? WHERE from_id=? AND to_id = ?;"; } else { - return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);"; + return "UPDATE Link SET type=?, transform=? WHERE from_id=? AND to_id = ?;"; + } +} +std::string DBDriverSqlite3::queryStepLink() const +{ + // from_id, to_id are at the end to match the update query above + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + return "INSERT INTO Link(type, rot_variance, trans_variance, transform, user_data, from_id, to_id) VALUES(?,?,?,?,?,?,?);"; + } + else if(uStrNumCmp(_version, "0.8.4") >= 0) + { + return "INSERT INTO Link(type, rot_variance, trans_variance, transform, from_id, to_id) VALUES(?,?,?,?,?,?);"; + } + else if(uStrNumCmp(_version, "0.7.4") >= 0) + { + return "INSERT INTO Link(type, variance, transform, from_id, to_id) VALUES(?,?,?,?,?);"; + } + else + { + return "INSERT INTO Link(type, transform, from_id, to_id) VALUES(?,?,?,?);"; } } void DBDriverSqlite3::stepLink( @@ -2522,10 +2662,6 @@ void DBDriverSqlite3::stepLink( int rc = SQLITE_OK; int index = 1; - rc = sqlite3_bind_int(ppStmt, index++, link.from()); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); - rc = sqlite3_bind_int(ppStmt, index++, link.to()); - UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); rc = sqlite3_bind_int(ppStmt, index++, link.type()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); @@ -2545,6 +2681,25 @@ void DBDriverSqlite3::stepLink( rc = sqlite3_bind_blob(ppStmt, index++, link.transform().data(), link.transform().size()*sizeof(float), SQLITE_STATIC); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + if(uStrNumCmp(_version, "0.10.10") >= 0) + { + // user_data + if(!link.userDataCompressed().empty()) + { + rc = sqlite3_bind_blob(ppStmt, index++, link.userDataCompressed().data, (int)link.userDataCompressed().cols, SQLITE_STATIC); + } + else + { + rc = sqlite3_bind_zeroblob(ppStmt, index++, 4); + } + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + } + + rc = sqlite3_bind_int(ppStmt, index++, link.from()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + rc = sqlite3_bind_int(ppStmt, index++, link.to()); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); + rc=sqlite3_step(ppStmt); UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index 38553d9a..80992141 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -64,6 +64,9 @@ private: virtual void updateQuery(const std::list & signatures, bool updateTimestamp) const; virtual void updateQuery(const std::list & words, bool updateTimestamp) const; + virtual void addLinkQuery(const Link & link) const; + virtual void updateLinkQuery(const Link & link) const; + // Load objects virtual void loadQuery(VWDictionary * dictionary) const; virtual void loadLastNodesQuery(std::list & signatures) const; @@ -85,6 +88,7 @@ private: std::string queryStepImage() const; std::string queryStepDepth() const; std::string queryStepSensorData() const; + std::string queryStepLinkUpdate() const; std::string queryStepLink() const; std::string queryStepWordsChanged() const; std::string queryStepKeypoint() const; diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index e28b7839..9c893449 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -45,11 +45,13 @@ namespace rtabmap { DBReader::DBReader(const std::string & databasePath, float frameRate, bool odometryIgnored, - bool ignoreGoalDelay) : + bool ignoreGoalDelay, + bool goalsIgnored) : _paths(uSplit(databasePath, ';')), _frameRate(frameRate), _odometryIgnored(odometryIgnored), _ignoreGoalDelay(ignoreGoalDelay), + _goalsIgnored(goalsIgnored), _dbDriver(0), _currentId(_ids.end()), _previousStamp(0) @@ -59,11 +61,13 @@ DBReader::DBReader(const std::string & databasePath, DBReader::DBReader(const std::list & databasePaths, float frameRate, bool odometryIgnored, - bool ignoreGoalDelay) : + bool ignoreGoalDelay, + bool goalsIgnored) : _paths(databasePaths), _frameRate(frameRate), _odometryIgnored(odometryIgnored), _ignoreGoalDelay(ignoreGoalDelay), + _goalsIgnored(goalsIgnored), _dbDriver(0), _currentId(_ids.end()), _previousStamp(0) @@ -155,8 +159,12 @@ void DBReader::mainLoop() { int goalId = 0; double previousStamp = odom.data().stamp(); - odom.data().setStamp(UTimer::now()); - if(odom.data().userDataRaw().type() == CV_8SC1 && + if(previousStamp == 0) + { + odom.data().setStamp(UTimer::now()); + } + if(!_goalsIgnored && + odom.data().userDataRaw().type() == CV_8SC1 && odom.data().userDataRaw().cols >= 7 && // including null str ending odom.data().userDataRaw().rows == 1 && memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0) @@ -205,19 +213,19 @@ void DBReader::mainLoop() double delay = stamp - previousStamp; UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...", goalId, delay); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); uSleep(delay*1000); } else { UWARN("Goal %d detected, posting it!", goalId); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); } } else { UWARN("Goal %d detected, posting it!", goalId); - this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId)); + this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId)); } } diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 34391c39..574591d2 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -48,10 +49,51 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/solvers/csparse/linear_solver_csparse.h" +#include "g2o/solvers/cholmod/linear_solver_cholmod.h" +#include "g2o/solvers/pcg/linear_solver_pcg.h" #include "g2o/types/slam3d/vertex_se3.h" #include "g2o/types/slam3d/edge_se3.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o/types/slam2d/edge_se2.h" + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; +typedef g2o::LinearSolverCSparse SlamLinearCSparseSolver; +typedef g2o::LinearSolverCholmod SlamLinearCholmodSolver; +typedef g2o::LinearSolverPCG SlamLinearPCGSolver; + +#include "vertigo/g2o/edge_switchPrior.h" +#include "vertigo/g2o/edge_se2Switchable.h" +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" + +#endif // end WITH_G2O + +#ifdef WITH_GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "vertigo/gtsam/betweenFactorMaxMix.h" +#include "vertigo/gtsam/betweenFactorSwitchable.h" +#include "vertigo/gtsam/switchVariableLinear.h" +#include "vertigo/gtsam/switchVariableSigmoid.h" +#endif // end WITH_GTSAM + +#ifdef WITH_CVSBA +#include +#include "rtabmap/core/util3d_motion_estimation.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/util3d_correspondences.h" #endif namespace rtabmap { @@ -73,16 +115,23 @@ Optimizer * Optimizer::create(const ParametersMap & parameters) UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; case Optimizer::kTypeTORO: default: optimizer = new TOROOptimizer(parameters); - type = Optimizer::kTypeTORO; break; } @@ -96,9 +145,17 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; @@ -112,11 +169,12 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para return optimizer; } -Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) : +Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) : iterations_(iterations), slam2d_(slam2d), covarianceIgnored_(covarianceIgnored), - epsilon_(epsilon) + epsilon_(epsilon), + robust_(robust) { } @@ -124,7 +182,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) : iterations_(Parameters::defaultRGBDOptimizeIterations()), slam2d_(Parameters::defaultRGBDOptimizeSlam2D()), covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()), - epsilon_(Parameters::defaultRGBDOptimizeEpsilon()) + epsilon_(Parameters::defaultRGBDOptimizeEpsilon()), + robust_(Parameters::defaultRGBDOptimizeRobust()) { parseParameters(parameters); } @@ -135,6 +194,27 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_); Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_); Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_); + Parameters::parse(parameters, Parameters::kRGBDOptimizeRobust(), robust_); +} + +std::map Optimizer::optimize( + int rootId, + const std::map & poses, + const std::multimap & constraints, + std::list > * intermediateGraphes) +{ + UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type()); + return std::map(); +} + +std::map Optimizer::optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures) +{ + UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type()); + return std::map(); } void Optimizer::getConnectedGraph( @@ -157,6 +237,14 @@ void Optimizer::getConnectedGraph( std::set nextDepth; nextDepth.insert(fromId); int d = 0; + std::multimap biLinks; + for(std::multimap::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter) + { + UASSERT_MSG(findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(), "Input links should be unique between two poses."); + biLinks.insert(std::make_pair(iter->second.from(), iter->second.to())); + biLinks.insert(std::make_pair(iter->second.to(), iter->second.from())); + } + while((depth == 0 || d < depth) && nextDepth.size()) { curentDepth = nextDepth; @@ -169,39 +257,22 @@ void Optimizer::getConnectedGraph( ids.insert(*jter); posesOut.insert(*posesIn.find(*jter)); - for(std::multimap::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter) + for(std::multimap::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter) { - if(iter->second.from() == *jter) + int nextId = iter->second; + if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId)) { - if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to())) - { - nextDepth.insert(iter->second.to()); - if(depth == 0 || d < depth-1) - { - linksOut.insert(*iter); - } - else if(curentDepth.find(iter->second.to()) != curentDepth.end() || - ids.find(iter->second.to()) != ids.end()) - { - linksOut.insert(*iter); - } - } - } - else if(iter->second.to() == *jter) - { - if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from())) - { - nextDepth.insert(iter->second.from()); + nextDepth.insert(nextId); - if(depth == 0 || d < depth-1) - { - linksOut.insert(*iter); - } - else if(curentDepth.find(iter->second.from()) != curentDepth.end() || - ids.find(iter->second.from()) != ids.end()) - { - linksOut.insert(*iter); - } + std::multimap::const_iterator kter = graph::findLink(linksIn, *jter, nextId); + if(depth == 0 || d < depth-1) + { + linksOut.insert(*kter); + } + else if(curentDepth.find(nextId) != curentDepth.end() || + ids.find(nextId) != ids.end()) + { + linksOut.insert(*kter); } } } @@ -346,9 +417,12 @@ std::map TOROOptimizer::optimize( pg3.initializeOptimization(); } - UINFO("TORO iterate begin (iterations=%d)", iterations()); + UINFO("TORO optimizing begin (iterations=%d)", iterations()); double lasterror = 0; - for (int i=0; i0) { @@ -402,7 +476,7 @@ std::map TOROOptimizer::optimize( } // early stop condition - double errorDelta = lasterror - error; + errorDelta = lasterror - error; if(i>0 && errorDelta < this->epsilon()) { UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); @@ -410,7 +484,7 @@ std::map TOROOptimizer::optimize( } lasterror = error; } - UINFO("TORO iterate end"); + UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, errorDelta, timer.ticks()); if(isSlam2d()) { @@ -643,20 +717,41 @@ std::map G2OOptimizer::optimize( { // Apply g2o optimization - // create the linear solver - g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCSparse(); - - // create the block solver on top of the linear solver - g2o::BlockSolverX* blockSolver = new g2o::BlockSolverX(linearSolver); - - // create the algorithm to carry out the optimization - //g2o::OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(blockSolver); - g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver); - - // create the optimizer to load the data and carry out the optimization g2o::SparseOptimizer optimizer; - optimizer.setVerbose(false); - optimizer.setAlgorithm(optimizationAlgorithm); + optimizer.setVerbose(ULogger::level()==ULogger::kDebug); + int solverApproach = 0; + int optimizationApproach = 1; + + SlamBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver(); + blockSolver = new SlamBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new SlamBlockSolver(linearSolver); + } + else + { + //chmold + SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new SlamBlockSolver(linearSolver); + } + + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } UDEBUG("fill poses to g2o..."); for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) @@ -667,16 +762,25 @@ std::map G2OOptimizer::optimize( { g2o::VertexSE2 * v2 = new g2o::VertexSE2(); v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta())); + if(iter->first == rootId) + { + v2->setFixed(true); + } vertex = v2; } else { g2o::VertexSE3 * v3 = new g2o::VertexSE3(); - Eigen::Isometry3d pose; + Eigen::Affine3d a = iter->second.toEigen3d(); + Eigen::Isometry3d pose; + pose = a.rotation(); pose.translation() = a.translation(); - pose.linear() = a.rotation(); v3->setEstimate(pose); + if(iter->first == rootId) + { + v3->setFixed(true); + } vertex = v3; } vertex->setId(iter->first); @@ -684,6 +788,7 @@ std::map G2OOptimizer::optimize( } UDEBUG("fill edges to g2o..."); + int vertigoVertexId = poses.rbegin()->first+1; for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { int id1 = iter->first; @@ -693,6 +798,32 @@ std::map G2OOptimizer::optimize( g2o::HyperGraph::Edge * edge = 0; + VertexSwitchLinear * v = 0; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + // For loop closure links, add switchable edges + + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + v = new VertexSwitchLinear(); + v->setEstimate(1.0); + v->setId(vertigoVertexId++); + UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str()); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + EdgeSwitchPrior * prior = new EdgeSwitchPrior(); + prior->setMeasurement(1.0); + prior->setVertex(0, v); + UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str()); + } + if(isSlam2d()) { Eigen::Matrix information = Eigen::Matrix::Identity(); @@ -709,16 +840,33 @@ std::map G2OOptimizer::optimize( information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } - g2o::EdgeSE2 * e = new g2o::EdgeSE2(); - g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); - g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE2Switchable * e = new EdgeSE2Switchable(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE2 * e = new g2o::EdgeSE2(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } } else { @@ -730,19 +878,36 @@ std::map G2OOptimizer::optimize( Eigen::Affine3d a = iter->second.transform().toEigen3d(); Eigen::Isometry3d constraint; + constraint = a.rotation(); constraint.translation() = a.translation(); - constraint.linear() = a.rotation(); - g2o::EdgeSE3 * e = new g2o::EdgeSE3(); - g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); - g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(constraint); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE3Switchable * e = new EdgeSE3Switchable(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE3 * e = new g2o::EdgeSE3(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } } if (!optimizer.addEdge(edge)) @@ -753,25 +918,13 @@ std::map G2OOptimizer::optimize( } UDEBUG("Initial optimization..."); - UASSERT(uContains(poses, rootId)); - if(isSlam2d()) - { - g2o::VertexSE2* firstRobotPose = (g2o::VertexSE2*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } - else - { - g2o::VertexSE3* firstRobotPose = (g2o::VertexSE3*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } + optimizer.initializeOptimization(); - UINFO("g2o iterate begin (max iterations=%d)", iterations()); + UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); int it = 0; + UTimer timer; if(intermediateGraphes) { - optimizer.initializeOptimization(); for(int i=0; i 0) @@ -820,18 +973,17 @@ std::map G2OOptimizer::optimize( if(ULogger::level() == ULogger::kDebug) { optimizer.computeActiveErrors(); - UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } } } else { - optimizer.initializeOptimization(); it = optimizer.optimize(iterations()); optimizer.computeActiveErrors(); - UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } - UINFO("g2o iterate end (%d iterations done)", it); + UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks()); if(isSlam2d()) { @@ -889,6 +1041,642 @@ std::map G2OOptimizer::optimize( return optimizedPoses; } +bool G2OOptimizer::saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints) +{ + FILE * file = 0; + +#ifdef _MSC_VER + fopen_s(&file, fileName.c_str(), "w"); +#else + file = fopen(fileName.c_str(), "w"); +#endif + + if(file) + { + // VERTEX_SE3 id x y z qw qx qy qz + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + Eigen::Quaternionf q = iter->second.getQuaternionf(); + fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n", + iter->first, + iter->second.x(), + iter->second.y(), + iter->second.z(), + q.x(), + q.y(), + q.z(), + q.w()); + } + + //EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 + int virtualVertexId = poses.size()?poses.rbegin()->first+1:0; + for(std::multimap::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + std::string prefix = "EDGE_SE3:QUAT"; + std::string suffix = ""; + + if(useRobustConstraints && iter->second.type() != Link::kNeighbor) + { + prefix = "EDGE_SE3_SWITCHABLE"; + fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId); + fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId); + suffix = uFormat(" %d", virtualVertexId++); + } + + Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); + fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", + prefix.c_str(), + iter->first, + iter->second.to(), + suffix.c_str(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().z(), + q.x(), + q.y(), + q.z(), + q.w(), + iter->second.infMatrix().at(0,0), + iter->second.infMatrix().at(0,1), + iter->second.infMatrix().at(0,2), + iter->second.infMatrix().at(0,3), + iter->second.infMatrix().at(0,4), + iter->second.infMatrix().at(0,5), + iter->second.infMatrix().at(1,1), + iter->second.infMatrix().at(1,2), + iter->second.infMatrix().at(1,3), + iter->second.infMatrix().at(1,4), + iter->second.infMatrix().at(1,5), + iter->second.infMatrix().at(2,2), + iter->second.infMatrix().at(2,3), + iter->second.infMatrix().at(2,4), + iter->second.infMatrix().at(2,5), + iter->second.infMatrix().at(3,3), + iter->second.infMatrix().at(3,4), + iter->second.infMatrix().at(3,5), + iter->second.infMatrix().at(4,4), + iter->second.infMatrix().at(4,5), + iter->second.infMatrix().at(5,5)); + } + UINFO("Graph saved to %s", fileName.c_str()); + fclose(file); + } + else + { + UERROR("Cannot save to file %s", fileName.c_str()); + return false; + } + return true; +} + +////////////////////// +// GTSAM +////////////////////// +bool GTSAMOptimizer::available() +{ +#ifdef WITH_GTSAM + return true; +#else + return false; +#endif +} + +std::map GTSAMOptimizer::optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes) +{ + std::map optimizedPoses; +#ifdef WITH_GTSAM + UDEBUG("Optimizing graph..."); + if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0) + { + gtsam::NonlinearFactorGraph graph; + + //prior first pose + UASSERT(uContains(poses, rootId)); + const Transform & initialPose = poses.at(rootId); + if(isSlam2d()) + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01)); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + } + else + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + } + + UDEBUG("fill poses to gtsam..."); + gtsam::Values initialEstimate; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + UASSERT(!iter->second.isNull()); + if(isSlam2d()) + { + initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); + } + else + { + initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); + } + } + + UDEBUG("fill edges to gtsam..."); + int switchCounter = poses.rbegin()->first+1; + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + int id1 = iter->first; + int id2 = iter->second.to(); + + UASSERT(!iter->second.transform().isNull()); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + double prior = 1.0; + initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + } + + if(isSlam2d()) + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x + information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y + information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta + information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x + information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y + information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta + information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x + information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y + information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta + } + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information = information / 1000.0; + } + + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + } + } + + UDEBUG("create optimizer"); + gtsam::GaussNewtonParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + //gtsam::LevenbergMarquardtParams parametersLev; + //parametersLev.relativeErrorTol = epsilon(); + //parametersLev.maxIterations = iterations(); + //gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev); + //gtsam::DoglegParams parametersDogleg; + //parametersDogleg.relativeErrorTol = epsilon(); + //parametersDogleg.maxIterations = iterations(); + //gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg); + + UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); + UTimer timer; + for(int i=0; i 0) + { + std::map tmpPoses; + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + intermediateGraphes->push_back(tmpPoses); + } + try + { + optimizer.iterate(); + } + catch(gtsam::IndeterminantLinearSystemException & e) + { + UERROR("GTSAM exception catched: %s", e.what()); + return optimizedPoses; + } + UDEBUG("iteration %d error =%f", i+1, optimizer.error()); + if(optimizer.error() < epsilon()) + { + break; + } + } + UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks()); + + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + } + else if(poses.size() == 1 || iterations() <= 0) + { + optimizedPoses = poses; + } + else + { + UWARN("This method should be called at least with 1 pose!"); + } + UDEBUG("Optimizing graph...end!"); +#else + UERROR("Not built with GTSAM support!"); +#endif + return optimizedPoses; +} + +////////////////////// +// cvsba +////////////////////// +bool CVSBAOptimizer::available() +{ +#ifdef WITH_CVSBA + return true; +#else + return false; +#endif +} + +std::map CVSBAOptimizer::optimizeBA( + int rootId, + const std::map & poses, + const std::multimap & links, + const std::map & signatures) +{ +#ifdef WITH_CVSBA + // run sba optimization + cvsba::Sba sba; + + // change params if desired + cvsba::Sba::Params params ; + params.type = cvsba::Sba::MOTIONSTRUCTURE; + params.iterations = this->iterations(); + params.minError = this->epsilon(); + params.fixedIntrinsics = 5; + params.fixedDistortion = 5; + params.verbose=ULogger::level() <= ULogger::kInfo; + sba.setParams(params); + + std::map frames = poses; + + std::vector cameraMatrix(frames.size()); //nframes + std::vector R(frames.size()); //nframes + std::vector T(frames.size()); //nframes + std::vector distCoeffs(frames.size()); //nframes + std::map frameIdToIndex; + std::map models; + int oi=0; + for(std::map::iterator iter=frames.begin(); iter!=frames.end(); ) + { + CameraModel model; + if(uContains(signatures, iter->first)) + { + if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValid()) + { + model = signatures.at(iter->first).sensorData().cameraModels()[0]; + } + else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValid()) + { + model = signatures.at(iter->first).sensorData().stereoCameraModel().left(); + } + else + { + UERROR("Missing calibration for node %d", iter->first); + } + } + else + { + UERROR("Did not find node %d in cache", iter->first); + } + + if(model.isValid()) + { + frameIdToIndex.insert(std::make_pair(iter->first, oi)); + + cameraMatrix[oi] = model.K(); + distCoeffs[oi] = model.D(); + + Transform t = (iter->second * model.localTransform()).inverse(); + + R[oi] = (cv::Mat_(3,3) << + (double)t.r11(), (double)t.r12(), (double)t.r13(), + (double)t.r21(), (double)t.r22(), (double)t.r23(), + (double)t.r31(), (double)t.r32(), (double)t.r33()); + T[oi] = (cv::Mat_(1,3) << (double)t.x(), (double)t.y(), (double)t.z()); + ++oi; + + models.insert(std::make_pair(iter->first, model)); + + UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str()); + + ++iter; + } + else + { + frames.erase(iter++); + } + } + cameraMatrix.resize(oi); + R.resize(oi); + T.resize(oi); + distCoeffs.resize(oi); + + std::map points3DMap; + std::multimap > wordReferences; // + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + Link link = iter->second; + if(link.to() < link.from()) + { + link = link.inverse(); + } + if(uContains(signatures, link.from()) && + uContains(signatures, link.to()) && + uContains(frames, link.from())) + { + const Signature & sFrom = signatures.at(link.from()); + const Signature & sTo = signatures.at(link.to()); + + std::vector inliers; + Transform t = util3d::estimateMotion3DTo3D( + uMultimapToMapUnique(sFrom.getWords3()), + uMultimapToMapUnique(sTo.getWords3()), + minInliers_, + inlierDistance_, + 100, + 10, + 0, + 0, + &inliers); + + if(!t.isNull()) + { + Transform pose = frames.at(sFrom.id()); + for(unsigned int i=0; isecond, pose); + std::map::iterator jter = points3DMap.find(inliers[i]); + if(jter == points3DMap.end()) + { + points3DMap.insert(std::make_pair(inliers[i], p)); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); + } + else + { + float dist = uNorm(p.x - jter->second.x, p.y - jter->second.y, p.z - jter->second.z); + if(dist <= inlierDistance_) + { + // in case of loop closure links + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(inliers[i])->second.pt))); + wordReferences.insert(std::make_pair(inliers[i], std::make_pair(sTo.id(), sTo.getWords().lower_bound(inliers[i])->second.pt))); + } + } + } + } + else + { + UWARN("Not enough inliers (%d) between %d and %d", inliers.size(), sFrom.id(), sTo.id()); + } + } + } + + std::list wordReferencesKeys = uUniqueKeys(wordReferences); + UDEBUG("points=%d frames=%d", (int)wordReferencesKeys.size(), (int)frames.size()); + std::vector points(wordReferencesKeys.size()); //npoints + std::vector > imagePoints(frames.size()); //nframes -> npoints + std::vector > visibility(frames.size()); //nframes -> npoints + for(unsigned int i=0; i::quiet_NaN(), std::numeric_limits::quiet_NaN())); + visibility[i].resize(wordReferencesKeys.size(), 0); + } + int i=0; + for(std::list::iterator iter = wordReferencesKeys.begin(); iter!=wordReferencesKeys.end(); ++iter) + { + pcl::PointXYZ & p = points3DMap.at(*iter); + points[i].x = p.x; + points[i].y = p.y; + points[i].z = p.z; + + std::multimap >::iterator jter = wordReferences.lower_bound(*iter); + while(jter->first == *iter && jter != wordReferences.end()) + { + imagePoints[frameIdToIndex.at(jter->second.first)][i] = jter->second.second; + visibility[frameIdToIndex.at(jter->second.first)][i] = 1; + ++jter; + } + + ++i; + } + + // SBA + try + { + sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs); + } + catch(cv::Exception & e) + { + UERROR("Running SBA... error! %s", e.what()); + return std::map(); + } + + //update poses + i=0; + for(std::map::iterator iter=frames.begin(); iter!=frames.end(); ++iter) + { + Transform t(R[i].at(0,0), R[i].at(0,1), R[i].at(0,2), T[i].at(0), + R[i].at(1,0), R[i].at(1,1), R[i].at(1,2), T[i].at(1), + R[i].at(2,0), R[i].at(2,1), R[i].at(2,2), T[i].at(2)); + + UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str()); + + iter->second = (models.at(iter->first).localTransform() * t).inverse(); + + ++i; + } + + return frames; + +#else + UERROR("RTAB-Map is not built with cvsba!"); + return std::map(); +#endif +} + +bool exportPoses( + const std::string & filePath, + int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o + const std::map & poses, + const std::multimap & constraints, // required for formats 3 and 4 + const std::map & stamps) // required for format 1 +{ + std::string tmpPath = filePath; + if(format==3) // TORO + { + if(UFile::getExtension(tmpPath).empty()) + { + tmpPath+=".graph"; + } + return graph::TOROOptimizer::saveGraph(tmpPath, poses, constraints); + } + else if(format == 4) // g2o + { + if(UFile::getExtension(tmpPath).empty()) + { + tmpPath+=".g2o"; + } +#ifdef WITH_G2O + return graph::G2OOptimizer::saveGraph(tmpPath, poses, constraints); +#else + UERROR("Cannot export in g2o format because RTAB-Map is not built with g2o support!"); + return false; +#endif + } + else + { + if(UFile::getExtension(tmpPath).empty()) + { + tmpPath+=".txt"; + } + + if(format == 1) + { + if(stamps.size() != poses.size()) + { + UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size!"); + return false; + } + } + + FILE* fout = 0; +#ifdef _MSC_VER + fopen_s(&fout, tmpPath.c_str(), "w"); +#else + fout = fopen(tmpPath.c_str(), "w"); +#endif + if(fout) + { + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(format == 1) // rgbd-slam format + { + // Format: stamp x y z qw qx qy qz + Eigen::Quaternionf q = (*iter).second.getQuaternionf(); + + UASSERT(uContains(stamps, iter->first)); + fprintf(fout, "%f %f %f %f %f %f %f %f\n", + stamps.at(iter->first), + (*iter).second.x(), + (*iter).second.y(), + (*iter).second.z(), + q.w(), + q.x(), + q.y(), + q.z()); + } + else // default / KITTI format + { + Transform pose = iter->second; + if(format == 2) + { + // for KITTI, we need to remove optical rotation + // z pointing front, x left, y down + Transform t( 0, 0, 1, 0, + -1, 0, 0, 0, + 0,-1, 0, 0); + pose = t.inverse() * pose * t; + } + + // Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz + const float * p = (const float *)pose.data(); + + fprintf(fout, "%f", p[0]); + for(int i=1; i > computePath( if(nodeIter == nodes.end()) { std::map::const_iterator poseIter = poses.find(iter->second); - UASSERT(poseIter != poses.end()); - Node n(iter->second, currentNode->id(), poseIter->second); - n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second)); - n.setDistToEnd(n.distFrom(endPose)); - nodes.insert(std::make_pair(iter->second, n)); - if(updateNewCosts) + if(poseIter == poses.end()) { - pqmap.insert(std::make_pair(n.totalCost(), n.id())); + UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first); } else { - pq.push(Pair(n.id(), n.totalCost())); + Node n(iter->second, currentNode->id(), poseIter->second); + n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second)); + n.setDistToEnd(n.distFrom(endPose)); + nodes.insert(std::make_pair(iter->second, n)); + if(updateNewCosts) + { + pqmap.insert(std::make_pair(n.totalCost(), n.id())); + } + else + { + pq.push(Pair(n.id(), n.totalCost())); + } } } else if(updateNewCosts && nodeIter->second.isOpened()) @@ -1322,12 +2117,21 @@ std::list > computePath( int toId, const Memory * memory, bool lookInDatabase, - bool updateNewCosts) + bool updateNewCosts, + float linearVelocity, // m/sec + float angularVelocity) // rad/sec { UASSERT(memory!=0); UASSERT(fromId>=0); UASSERT(toId>=0); std::list > path; + UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f", + fromId, + toId, + lookInDatabase?1:0, + updateNewCosts?1:0, + linearVelocity, + angularVelocity); std::multimap allLinks; if(lookInDatabase) @@ -1398,11 +2202,34 @@ std::list > computePath( } for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) { + Transform nextPose = currentNode->pose()*iter->second.transform(); + float cost = 0.0f; + if(linearVelocity <= 0.0f && angularVelocity <= 0.0f) + { + // use distance only + cost = iter->second.transform().getNorm(); + } + else // use time + { + if(linearVelocity > 0.0f) + { + cost += iter->second.transform().getNorm()/linearVelocity; + } + if(angularVelocity > 0.0f) + { + Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f); + Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1); + float angle = pcl::getAngle3D(v1, v2); + cost += angle / angularVelocity; + } + } + std::map::iterator nodeIter = nodes.find(iter->first); if(nodeIter == nodes.end()) { - Node n(iter->second.to(), currentNode->id(), currentNode->pose()*iter->second.transform()); - n.setCostSoFar(currentNode->costSoFar() + iter->second.transform().getNorm()); + Node n(iter->second.to(), currentNode->id(), nextPose); + + n.setCostSoFar(currentNode->costSoFar() + cost); nodes.insert(std::make_pair(iter->second.to(), n)); if(updateNewCosts) { @@ -1415,9 +2242,12 @@ std::list > computePath( } else if(updateNewCosts && nodeIter->second.isOpened()) { - float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose()); + float newCostSoFar = currentNode->costSoFar() + cost; if(nodeIter->second.costSoFar() > newCostSoFar) { + // update pose with new link + nodeIter->second.setPose(nextPose); + // update the cost in the priority queue for(std::multimap::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter) { @@ -1433,6 +2263,61 @@ std::list > computePath( } } } + + // Debugging stuff + if(ULogger::level() == ULogger::kDebug) + { + std::stringstream stream; + std::vector linkTypes(Link::kUndef, 0); + std::list >::const_iterator previousIter = path.end(); + float length = 0.0f; + for(std::list >::const_iterator iter=path.begin(); iter!=path.end();++iter) + { + if(iter!=path.begin()) + { + stream << ","; + } + + if(previousIter!=path.end()) + { + //UDEBUG("current %d = %s", iter->first, iter->second.prettyPrint().c_str()); + if(allLinks.size()) + { + std::multimap::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first); + if(jter != allLinks.end()) + { + //Transform nextPose = iter->second; + //Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f); + //Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1); + //float angle = pcl::getAngle3D(v1, v2); + //float cost = angle ; + //UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost); + + UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()second.type()]; + stream << "[" << jter->second.type() << "]"; + length += jter->second.transform().getNorm(); + } + } + } + + stream << iter->first; + + previousIter=iter; + } + UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str()); + std::stringstream streamB; + for(unsigned int i=0; i 0) + { + streamB << " "; + } + streamB << i << "=" << linkTypes[i]; + } + UDEBUG("Link types = %s", streamB.str().c_str()); + } + return path; } @@ -1603,6 +2488,39 @@ float computePathLength( return length; } +// return all paths linked only by neighbor links +std::list > getPaths( + std::map poses, + const std::multimap & links) +{ + std::list > paths; + if(poses.size() && links.size()) + { + // Segment poses connected only by neighbor links + while(poses.size()) + { + std::map path; + for(std::map::iterator iter=poses.begin(); iter!=poses.end();) + { + std::multimap::const_iterator jter = findLink(links, path.rbegin()->first, iter->first); + if(path.size() == 0 || (jter != links.end() && jter->second.type() == Link::kNeighbor)) + { + path.insert(*iter); + poses.erase(iter++); + } + else + { + break; + } + } + UASSERT(path.size()); + paths.push_back(path); + } + + } + return paths; +} + } /* namespace graph */ } /* namespace rtabmap */ diff --git a/corelib/src/Link.cpp b/corelib/src/Link.cpp new file mode 100644 index 00000000..7ba28f9b --- /dev/null +++ b/corelib/src/Link.cpp @@ -0,0 +1,198 @@ +/* +Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "rtabmap/core/Link.h" +#include +#include +#include + +namespace rtabmap { + +Link::Link() : + from_(0), + to_(0), + type_(kUndef), + infMatrix_(cv::Mat::eye(6,6,CV_64FC1)) +{ +} +Link::Link(int from, + int to, + Type type, + const Transform & transform, + const cv::Mat & infMatrix, + const cv::Mat & userData) : + from_(from), + to_(to), + transform_(transform), + type_(type) +{ + setInfMatrix(infMatrix); + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } +} +Link::Link(int from, + int to, + Type type, + const Transform & transform, + double rotVariance, + double transVariance, + const cv::Mat & userData) : + from_(from), + to_(to), + transform_(transform), + type_(type) +{ + setVariance(rotVariance, transVariance); + + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + } +} + +double Link::rotVariance() const +{ + double min = uMin3(infMatrix_.at(3,3), infMatrix_.at(4,4), infMatrix_.at(5,5)); + UASSERT(min > 0.0); + return 1.0/min; +} +double Link::transVariance() const +{ + double min = uMin3(infMatrix_.at(0,0), infMatrix_.at(1,1), infMatrix_.at(2,2)); + UASSERT(min > 0.0); + return 1.0/min; +} + +void Link::setInfMatrix(const cv::Mat & infMatrix) { + UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1); + UASSERT_MSG(uIsFinite(infMatrix.at(0,0)) && infMatrix.at(0,0)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(1,1)) && infMatrix.at(1,1)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(2,2)) && infMatrix.at(2,2)>0, "Transitional information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(3,3)) && infMatrix.at(3,3)>0, "Rotational information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(4,4)) && infMatrix.at(4,4)>0, "Rotational information should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(infMatrix.at(5,5)) && infMatrix.at(5,5)>0, "Rotational information should not be null! (set to 1 if unknown)"); + infMatrix_ = infMatrix; +} +void Link::setVariance(double rotVariance, double transVariance) { + UASSERT(uIsFinite(rotVariance) && rotVariance>0); + UASSERT(uIsFinite(transVariance) && transVariance>0); + infMatrix_ = cv::Mat::eye(6,6,CV_64FC1); + infMatrix_.at(0,0) = 1.0/transVariance; + infMatrix_.at(1,1) = 1.0/transVariance; + infMatrix_.at(2,2) = 1.0/transVariance; + infMatrix_.at(3,3) = 1.0/rotVariance; + infMatrix_.at(4,4) = 1.0/rotVariance; + infMatrix_.at(5,5) = 1.0/rotVariance; +} + +void Link::setUserDataRaw(const cv::Mat & userDataRaw) +{ + if(!_userDataRaw.empty()) + { + UWARN("Writing new user data over existing user data. This may result in data loss."); + } + _userDataRaw = userDataRaw; +} + +void Link::setUserData(const cv::Mat & userData) +{ + if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty())) + { + UWARN("Writing new user data over existing user data. This may result in data loss."); + } + _userDataRaw = cv::Mat(); + _userDataCompressed = cv::Mat(); + + if(!userData.empty()) + { + if(userData.type() == CV_8UC1) // Bytes + { + _userDataCompressed = userData; // assume compressed + } + else + { + _userDataRaw = userData; + _userDataCompressed = compressData2(userData); + } + } +} + +void Link::uncompressUserData() +{ + cv::Mat dataRaw = uncompressUserDataConst(); + if(!dataRaw.empty() && _userDataRaw.empty()) + { + _userDataRaw = dataRaw; + } +} + +cv::Mat Link::uncompressUserDataConst() const +{ + if(!_userDataRaw.empty()) + { + return _userDataRaw; + } + return uncompressData(_userDataCompressed); +} + +Link Link::merge(const Link & link, Type outputType) const +{ + UASSERT(to_ == link.from()); + UASSERT(outputType != Link::kUndef); + UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull())); + UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1); + UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1); + return Link( + from_, + link.to(), + outputType, + transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2) + transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_ + link.infMatrix()); +} + +Link Link::inverse() const +{ + return Link( + to_, + from_, + type_, + transform_.isNull()?Transform():transform_.inverse(), + transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_); +} + +} diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 178f02bd..ff6302b7 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -38,9 +38,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/VWDictionary.h" #include -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/Features2d.h" -#include "DBDriverSqlite3.h" +#include "rtabmap/core/DBDriver.h" #include "rtabmap/core/util3d_features.h" #include "rtabmap/core/util3d_filtering.h" #include "rtabmap/core/util3d_correspondences.h" @@ -48,7 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/util3d_surface.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/core/util3d_motion_estimation.h" -#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/Statistics.h" #include "rtabmap/core/Compression.h" @@ -68,6 +68,7 @@ Memory::Memory(const ParametersMap & parameters) : _similarityThreshold(Parameters::defaultMemRehearsalSimilarity()), _rawDataKept(Parameters::defaultMemImageKept()), _binDataKept(Parameters::defaultMemBinDataKept()), + _saveDepth16Format(Parameters::defaultMemSaveDepth16Format()), _notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()), _incrementalMemory(Parameters::defaultMemIncrementalMemory()), _maxStMemSize(Parameters::defaultMemSTMSize()), @@ -107,6 +108,7 @@ Memory::Memory(const ParametersMap & parameters) : _bowEstimationType(Parameters::defaultLccBowEstimationType()), _bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()), _bowPnPFlags(Parameters::defaultLccBowPnPFlags()), + _bowVarianceFromInliersCount(Parameters::defaultLccBowVarianceFromInliersCount()), _icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()), _icpMaxRotation(Parameters::defaultLccIcpMaxRotation()), @@ -182,7 +184,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter if(_dbDriver == 0 && !dbUrl.empty()) { - _dbDriver = new DBDriverSqlite3(parameters); + _dbDriver = DBDriver::create(parameters); } bool success = true; @@ -399,6 +401,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept); Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept); + Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format); Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb); Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal); Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds); @@ -418,6 +421,8 @@ void Memory::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str()); UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str()); UASSERT(_imageDecimation >= 1); + UASSERT(_rehearsalMaxDistance >= 0.0f); + UASSERT(_rehearsalMaxAngle >= 0.0f); // SLAM mode vs Localization mode iter = parameters.find(Parameters::kMemIncrementalMemory()); @@ -446,6 +451,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar); Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError); Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags); + Parameters::parse(parameters, Parameters::kLccBowVarianceFromInliersCount(), _bowVarianceFromInliersCount); Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation); Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation); Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation); @@ -662,7 +668,6 @@ bool Memory::update( } _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); - ++_signaturesAdded; } if(!_memoryChanged && _incrementalMemory) @@ -764,6 +769,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance _signatures.insert(_signatures.end(), std::pair(signature->id(), signature)); _stMem.insert(_stMem.end(), signature->id()); + ++_signaturesAdded; if(_vwd) { @@ -986,7 +992,7 @@ std::map Memory::getNeighborsId(int signatureId, ids.insert(std::pair(*jter, m)); UTimer timer; - _dbDriver->loadLinks(*jter, tmpLinks); + _dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kNeighbor:Link::kUndef); if(dbAccessTime) { *dbAccessTime += timer.getElapsedTime(); @@ -1084,7 +1090,8 @@ std::map Memory::getNeighborsIdRadius( for(std::map::const_iterator iter=links->begin(); iter!=links->end(); ++iter) { if(!uContains(ids, iter->first) && - uContains(optimizedPoses, iter->first)) + uContains(optimizedPoses, iter->first) && + iter->second.type()!=Link::kVirtualClosure) { const Transform & t = optimizedPoses.at(iter->first); UASSERT(!t.isNull()); @@ -1443,7 +1450,7 @@ std::list Memory::forget(const std::set & ignoredIds) { UDEBUG(""); std::list signaturesRemoved; - if(_vwd->isIncremental() && _vwd->getVisualWords().size()) + if(this->isIncremental() && _vwd->isIncremental() && _vwd->getVisualWords().size()) { int newWords = 0; int wordsRemoved = 0; @@ -1481,7 +1488,8 @@ std::list Memory::forget(const std::set & ignoredIds) { UDEBUG(""); // Remove one more than total added during the iteration - std::list signatures = getRemovableSignatures(_signaturesAdded+1, ignoredIds); + int signaturesAdded = _signaturesAdded; + std::list signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds); for(std::list::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter) { signaturesRemoved.push_back((*iter)->id()); @@ -1489,7 +1497,15 @@ std::list Memory::forget(const std::set & ignoredIds) // and it is removed from the memory list this->moveToTrash(*iter); } - UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), _signaturesAdded); + if((int)signatures.size() < signaturesAdded) + { + UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.", + (int)signatures.size(), signaturesAdded); + } + else + { + UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded); + } } return signaturesRemoved; } @@ -1648,42 +1664,39 @@ std::list Memory::getRemovableSignatures(int count, const std::set< int recentWmCount = 0; // make the list of removable signatures // Criteria : Weight -> ID - UDEBUG("signatureMap.size()=%d", (int)weightAgeIdMap.size()); + UDEBUG("signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d", + (int)weightAgeIdMap.size(), _lastGlobalLoopClosureId, currentRecentWmSize, recentWmMaxSize); for(std::map::iterator iter=weightAgeIdMap.begin(); iter!=weightAgeIdMap.end(); ++iter) { - bool removable = true; - if(removable) + if(!recentWmImmunized) { - if(!recentWmImmunized) - { - UDEBUG("weight=%d, id=%d", - iter->second->getWeight(), - iter->second->id()); - removableSignatures.push_back(iter->second); + UDEBUG("weight=%d, id=%d", + iter->second->getWeight(), + iter->second->id()); + removableSignatures.push_back(iter->second); - if(iter->second->id() > _lastGlobalLoopClosureId) + if(_lastGlobalLoopClosureId && iter->second->id() > _lastGlobalLoopClosureId) + { + ++recentWmCount; + if(currentRecentWmSize - recentWmCount < recentWmMaxSize) { - ++recentWmCount; - if(currentRecentWmSize - recentWmCount < recentWmMaxSize) - { - UDEBUG("switched recentWmImmunized"); - recentWmImmunized = true; - } + UDEBUG("switched recentWmImmunized"); + recentWmImmunized = true; } } - else if(iter->second->id() < _lastGlobalLoopClosureId) - { - UDEBUG("weight=%d, id=%d", - iter->second->getWeight(), - iter->second->id()); - removableSignatures.push_back(iter->second); - } - if(removableSignatures.size() >= (unsigned int)count) - { - break; - } + } + else if(_lastGlobalLoopClosureId == 0 || iter->second->id() < _lastGlobalLoopClosureId) + { + UDEBUG("weight=%d, id=%d", + iter->second->getWeight(), + iter->second->id()); + removableSignatures.push_back(iter->second); + } + if(removableSignatures.size() >= (unsigned int)count) + { + break; } } } @@ -1770,6 +1783,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * _workingMem.erase(s->id()); _stMem.erase(s->id()); _signatures.erase(s->id()); + if(_signaturesAdded>0) + { + --_signaturesAdded; + } if(_lastSignature == s) { @@ -2193,6 +2210,11 @@ Transform Memory::computeVisualTransform( } } + if(_bowVarianceFromInliersCount) + { + variance = inliersCount > 0?1.0/double(inliersCount):1.0; + } + if(rejectedMsg) { *rejectedMsg = msg; @@ -2203,7 +2225,7 @@ Transform Memory::computeVisualTransform( } if(varianceOut) { - *varianceOut = variance; + *varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform } UDEBUG("transform=%s", transform.prettyPrint().c_str()); return transform; @@ -2346,7 +2368,7 @@ Transform Memory::computeIcpTransform( bool hasConverged = false; Transform icpT; int correspondences = 0; - float correspondencesRatio = -1.0f; + float correspondencesRatio = 0.0f; double variance = 1; if(_icpPointToPlane) { @@ -2407,7 +2429,7 @@ Transform Memory::computeIcpTransform( if(varianceOut) { - *varianceOut = variance; + *varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform } if(correspondencesOut) { @@ -2495,9 +2517,9 @@ Transform Memory::computeIcpTransform( { Transform icpT; bool hasConverged = false; - float correspondencesRatio = -1.0f; + float correspondencesRatio = 0.0f; int correspondences = 0; - double variance = 1; + double variance = 1.0; pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud()); icpT = util3d::icp2D( newCloudVoxelized, @@ -2546,7 +2568,6 @@ Transform Memory::computeIcpTransform( newCloud = newCloudRegistered; } - pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); util3d::computeVarianceAndCorrespondences( newCloud, oldCloud, @@ -2561,8 +2582,9 @@ Transform Memory::computeIcpTransform( } else { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", newS.id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()); } UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", @@ -2570,12 +2592,12 @@ Transform Memory::computeIcpTransform( hasConverged?"true":"false", variance, correspondences, - (int)(newS.sensorData().laserScanMaxPts()), + newS.sensorData().laserScanMaxPts()?newS.sensorData().laserScanMaxPts():(int)(newCloud->size()>oldCloud->size()?newCloud->size():oldCloud->size()), correspondencesRatio*100.0f); if(varianceOut) { - *varianceOut = variance; + *varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform } if(correspondencesOut) { @@ -2605,6 +2627,21 @@ Transform Memory::computeIcpTransform( hasConverged?"true":"false", variance); UINFO(msg.c_str()); } + + // still compute the variance for information + if(variance == 1 && varianceOut) + { + util3d::computeVarianceAndCorrespondences( + newCloudVoxelized, + oldCloudVoxelized, + _icpMaxCorrespondenceDistance, + variance, + correspondences); + if(variance > 0) + { + *varianceOut = variance; + } + } } else { @@ -2726,64 +2763,83 @@ Transform Memory::computeScanMatchingTransform( if(!icpT.isNull() && hasConverged) { - if(_icp2VoxelSize <= _laserScanVoxelSize) + float ix,iy,iz, iroll,ipitch,iyaw; + icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); + if((_icpMaxTranslation>0.0f && + (fabs(ix) > _icpMaxTranslation || + fabs(iy) > _icpMaxTranslation || + fabs(iz) > _icpMaxTranslation)) + || + (_icpMaxRotation>0.0f && + (fabs(iroll) > _icpMaxRotation || + fabs(ipitch) > _icpMaxRotation || + fabs(iyaw) > _icpMaxRotation))) { - newCloud = util3d::transformPointCloud(newCloud, icpT); - } - else - { - newCloud = newCloudRegistered; - } - - pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); - double v = 1; - util3d::computeVarianceAndCorrespondences( - newCloud, - assembledOldClouds, - _icpMaxCorrespondenceDistance, - v, - correspondences); - if(variance) - { - *variance = v; - } - - // verify if there enough correspondences - float correspondencesRatio = 0.0f; - if(newS->sensorData().laserScanMaxPts()) - { - correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); - } - else - { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", - newS->id()); - } - - UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio*100.0f); - - if(inliers) - { - *inliers = correspondences; - } - - if(correspondencesRatio >= _icp2CorrespondenceRatio) - { - transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); - } - else - { - msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio); + msg = uFormat("Cannot compute transform (ICP correction too large)"); UINFO(msg.c_str()); } + else + { + if(_icp2VoxelSize <= _laserScanVoxelSize) + { + newCloud = util3d::transformPointCloud(newCloud, icpT); + } + else + { + newCloud = newCloudRegistered; + } + + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + double v = 1; + util3d::computeVarianceAndCorrespondences( + newCloud, + assembledOldClouds, + _icpMaxCorrespondenceDistance, + v, + correspondences); + if(variance) + { + *variance = v>0.0f?v:0.0001; // epsilon if exact transform + } + + // verify if there enough correspondences + float correspondencesRatio = 0.0f; + if(newS->sensorData().laserScanMaxPts()) + { + correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); + } + else + { + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!", + newS->id()); + correspondencesRatio = float(correspondences)/float(newCloud->size()); + } + + UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", + v, + correspondences, + newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(), + correspondencesRatio*100.0f); + + if(inliers) + { + *inliers = correspondences; + } + + if(correspondencesRatio >= _icp2CorrespondenceRatio) + { + transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); + } + else + { + msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)", + variance?*variance:-1, + correspondences, + newS->sensorData().laserScanMaxPts()?newS->sensorData().laserScanMaxPts():(int)newCloud->size(), + correspondencesRatio); + UINFO(msg.c_str()); + } + } } else { @@ -3087,7 +3143,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) } //============================================================ - // Compare with the last (not null) + // Compare with the last (not intermediate node) //============================================================ Signature * sB = 0; for(std::set::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter) @@ -3112,55 +3168,9 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) { if(_incrementalMemory) { - if(signature->hasLink(id)) + if(this->rehearsalMerge(id, signature->id())) { - if(signature->getLinks().begin()->second.transform().isNull()) - { - if(this->rehearsalMerge(id, signature->id())) - { - merged = id; - } - } - else - { - float x,y,z, roll,pitch,yaw; - signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if((_rehearsalMaxDistance>0.0f && ( - fabs(x) > _rehearsalMaxDistance || - fabs(y) > _rehearsalMaxDistance || - fabs(z) > _rehearsalMaxDistance)) || - (_rehearsalMaxAngle>0.0f && ( - fabs(roll) > _rehearsalMaxAngle || - fabs(pitch) > _rehearsalMaxAngle || - fabs(yaw) > _rehearsalMaxAngle))) - { - if(_rehearsalWeightIgnoredWhileMoving) - { - UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad", - _rehearsalMaxDistance, _rehearsalMaxAngle); - } - else - { - // if the robot has moved, increase only weight of the new one - signature->setWeight(sB->getWeight() + signature->getWeight() + 1); - sB->setWeight(0); - UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)", - signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle); - } - } - else if(this->rehearsalMerge(id, signature->id())) - { - merged = id; - } - } - } - else - { - // cannot merge not neighbor signatures, just update weight - signature->setWeight(sB->getWeight() + signature->getWeight() + 1); - sB->setWeight(0); - UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.", - signature->getWeight(), signature->id(), sB->id()); + merged = id; } } else @@ -3171,6 +3181,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats) if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged); if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim); + if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_id(), sim >= _similarityThreshold?id:0); UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks()); } else @@ -3198,65 +3209,122 @@ bool Memory::rehearsalMerge(int oldId, int newId) UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id()); - //remove mutual links - oldS->removeLink(newId); - newS->removeLink(oldId); - - if(_idUpdatedToNewOneRehearsal) + bool fullMerge; + bool intermediateMerge = false; + if(!newS->getLinks().begin()->second.transform().isNull()) { - // redirect neighbor links - const std::map & links = oldS->getLinks(); - for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + // we are in metric SLAM mode: + // 1) Normal merge if not moving AND has direct link + // 2) Transform to intermediate node (weight = -1) if not moving AND hasn't direct link. + float x,y,z, roll,pitch,yaw; + newS->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + bool isMoving = fabs(x) > _rehearsalMaxDistance || + fabs(y) > _rehearsalMaxDistance || + fabs(z) > _rehearsalMaxDistance || + fabs(roll) > _rehearsalMaxAngle || + fabs(pitch) > _rehearsalMaxAngle || + fabs(yaw) > _rehearsalMaxAngle; + if(isMoving && _rehearsalWeightIgnoredWhileMoving) { - Link link = iter->second; - link.setFrom(newS->id()); - - Signature * s = this->_getSignature(link.to()); - if(s) - { - // modify neighbor "from" - s->changeLinkIds(oldS->id(), newS->id()); - - newS->addLink(link); - } - else - { - UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); - } - } - newS->setLabel(oldS->getLabel()); - oldS->setLabel(""); - oldS->removeLinks(); // remove all links - oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location - - // Set old image to new signature - this->copyData(oldS, newS); - - // update weight - newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - - if(_lastGlobalLoopClosureId == oldS->id()) - { - _lastGlobalLoopClosureId = newS->id(); + UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)", + _rehearsalMaxDistance, _rehearsalMaxAngle); + return false; } + fullMerge = !isMoving && newS->hasLink(oldS->id()); + intermediateMerge = !isMoving && !newS->hasLink(oldS->id()); } else { - newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location - - // update weight - oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); - - if(_lastSignature == newS) - { - _lastSignature = oldS; - } + fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull(); } - // remove location - moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); + if(fullMerge) + { + //remove mutual links + Link newToOldLink = newS->getLinks().at(oldS->id()); + oldS->removeLink(newId); + newS->removeLink(oldId); - return true; + if(_idUpdatedToNewOneRehearsal) + { + // redirect neighbor links + const std::map & links = oldS->getLinks(); + for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) + { + Link link = iter->second; + Link mergedLink = newToOldLink.merge(link, link.type()); + UASSERT(mergedLink.from() == newS->id() && mergedLink.to() == link.to()); + + Signature * s = this->_getSignature(link.to()); + if(s) + { + // modify neighbor "from" + s->removeLink(oldS->id()); + s->addLink(mergedLink.inverse()); + + newS->addLink(mergedLink); + } + else + { + UERROR("Didn't find neighbor %d of %d in RAM...", link.to(), oldS->id()); + } + } + newS->setLabel(oldS->getLabel()); + oldS->setLabel(""); + oldS->removeLinks(); // remove all links + oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1, 1)); // to keep track of the merged location + + // Set old image to new signature + this->copyData(oldS, newS); + + // update weight + newS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); + + if(_lastGlobalLoopClosureId == oldS->id()) + { + _lastGlobalLoopClosureId = newS->id(); + } + } + else + { + newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform() , 1, 1)); // to keep track of the merged location + + // update weight + oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight()); + + if(_lastSignature == newS) + { + _lastSignature = oldS; + } + } + + // remove location + moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); + + return true; + } + else + { + // update only weights + if(_idUpdatedToNewOneRehearsal) + { + // just update weight + int w = oldS->getWeight()>=0?oldS->getWeight():0; + newS->setWeight(w + newS->getWeight() + 1); + oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + + if(_lastGlobalLoopClosureId == oldS->id()) + { + _lastGlobalLoopClosureId = newS->id(); + } + } + else // !_idUpdatedToNewOneRehearsal + { + int w = newS->getWeight()>=0?newS->getWeight():0; + oldS->setWeight(w + oldS->getWeight() + 1); + newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node + } + } } else { @@ -3324,23 +3392,27 @@ cv::Mat Memory::getImageCompressed(int signatureId) const return image; } -SensorData Memory::getNodeData(int nodeId, bool uncompressedData) +SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoadedDataInMemory) { UDEBUG("nodeId=%d", nodeId); SensorData r; Signature * s = this->_getSignature(nodeId); if(s && !s->sensorData().imageCompressed().empty()) { - if(uncompressedData) + if(keepLoadedDataInMemory && uncompressedData) { s->sensorData().uncompressData(); } r = s->sensorData(); + if(!keepLoadedDataInMemory && uncompressedData) + { + r.uncompressData(); + } } else if(_dbDriver) { // load from database - if(s) + if(s && keepLoadedDataInMemory) { std::list signatures; signatures.push_back(s); @@ -3451,7 +3523,7 @@ SensorData Memory::getSignatureDataConst(int locationId) const return r; } -void Memory::generateGraph(const std::string & fileName, std::set ids) +void Memory::generateGraph(const std::string & fileName, const std::set & ids) { if(!_dbDriver) { @@ -3459,235 +3531,7 @@ void Memory::generateGraph(const std::string & fileName, std::set ids) return; } - if(!fileName.empty()) - { - FILE* fout = 0; - #ifdef _MSC_VER - fopen_s(&fout, fileName.c_str(), "w"); - #else - fout = fopen(fileName.c_str(), "w"); - #endif - - if (!fout) - { - UERROR("Cannot open file %s!", fileName.c_str()); - return; - } - - if(ids.size() == 0) - { - _dbDriver->getAllNodeIds(ids); - UDEBUG("ids.size()=%d", ids.size()); - for(std::map::iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter) - { - ids.insert(iter->first); - } - } - - const char * colorG = "green"; - const char * colorP = "pink"; -; UINFO("Generating map with %d locations", ids.size()); - fprintf(fout, "digraph G {\n"); - for(std::set::iterator i=ids.begin(); i!=ids.end(); ++i) - { - if(_signatures.find(*i) == _signatures.end()) - { - int id = *i; - std::map links; - _dbDriver->loadLinks(id, links); - int weight = 0; - _dbDriver->getWeight(id, weight); - for(std::map::iterator iter = links.begin(); iter!=links.end(); ++iter) - { - int weightNeighbor = 0; - if(_signatures.find(iter->first) == _signatures.end()) - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - else - { - weightNeighbor = _signatures.find(iter->first)->second->getWeight(); - } - //UDEBUG("Add neighbor link from %d to %d", id, iter->first); - if(iter->second.type() == Link::kNeighbor) - { - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", - id, - weight, - iter->first, - weightNeighbor); - } - else if(iter->first > id) - { - //loop - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorG); - } - else - { - //child - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorP); - } - } - } - } - for(std::map::iterator i=_signatures.begin(); i!=_signatures.end(); ++i) - { - if(ids.find(i->first) != ids.end()) - { - int id = i->second->id(); - const std::map & links = i->second->getLinks(); - int weight = i->second->getWeight(); - for(std::map::const_iterator iter = links.begin(); iter!=links.end(); ++iter) - { - int weightNeighbor = 0; - const Signature * s = this->getSignature(iter->first); - if(s) - { - weightNeighbor = s->getWeight(); - } - else - { - _dbDriver->getWeight(iter->first, weightNeighbor); - } - //UDEBUG("Add neighbor link from %d to %d", id, iter->first); - if(iter->second.type() == Link::kNeighbor) - { - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\"\n", - id, - weight, - iter->first, - weightNeighbor); - } - else if(iter->first > id) - { - //loop - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"L\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorG); - } - else - { - //child - fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"C\", fontcolor=%s, fontsize=8];\n", - id, - weight, - iter->first, - weightNeighbor, - colorP); - } - } - } - } - fprintf(fout, "}\n"); - fclose(fout); - UINFO("Graph saved to \"%s\"", fileName.c_str()); - } -} - -// Only used to generate a .dot file -class GraphNode -{ -public: - GraphNode(int id, GraphNode * parent = 0) : - _parent(parent), - _id(id) - { - if(_parent) - { - _parent->addChild(this); - } - } - virtual ~GraphNode() - { - //We copy the set because when a child is destroyed, it is removed from its parent. - std::set children = _children; - _children.clear(); - for(std::set::iterator iter=children.begin(); iter!=children.end(); ++iter) - { - delete *iter; - } - children.clear(); - if(_parent) - { - _parent->removeChild(this); - } - } - int id() const {return _id;} - bool isAncestor(int id) const - { - if(_parent) - { - if(_parent->id() == id) - { - return true; - } - return _parent->isAncestor(id); - } - return false; - } - - void expand(std::list > & paths, std::list currentPath = std::list()) const - { - currentPath.push_back(_id); - if(_children.size() == 0) - { - paths.push_back(currentPath); - return; - } - for(std::set::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter) - { - (*iter)->expand(paths, currentPath); - } - } - -private: - void addChild(GraphNode * child) - { - _children.insert(child); - } - void removeChild(GraphNode * child) - { - _children.erase(child); - } - -private: - std::set _children; - GraphNode * _parent; - int _id; -}; - -//recursive -void Memory::createGraph(GraphNode * parent, unsigned int maxDepth, const std::set & endIds) -{ - if(maxDepth == 0 || !parent) - { - return; - } - std::map neighbors = this->getNeighborsId(parent->id(), 1, -1, false); - for(std::map::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) - { - if(!parent->isAncestor(iter->first)) - { - GraphNode * n = new GraphNode(iter->first, parent); - if(endIds.find(iter->first) == endIds.end()) - { - this->createGraph(n, maxDepth-1, endIds); - } - } - } + _dbDriver->generateGraph(fileName, ids, _signatures); } int Memory::getNi(int signatureId) const @@ -3831,6 +3675,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p if(_parallelized) { + UDEBUG("Start dictionary update thread"); preUpdateThread.start(); } @@ -3844,12 +3689,14 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p // convert to grayscale if(data.imageRaw().channels() > 1) { + UDEBUG("convert to grayscale..."); cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY); } else { imageMono = data.imageRaw(); } + UDEBUG("Set ROI..."); cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios); if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid()) @@ -3861,6 +3708,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p { subPixelOn = true; } + UDEBUG("Generating keypoints..."); keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); @@ -3947,6 +3795,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p { subPixelOn = true; } + UDEBUG("Generating keypoints..."); keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); @@ -4008,6 +3857,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p else { //RGB only + UDEBUG("Generating keypoints..."); keypoints = _feature2D->generateKeypoints(imageMono, roi); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f); @@ -4127,7 +3977,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p if(_parallelized) { + UDEBUG("Joining dictionary update thread..."); preUpdateThread.join(); // Wait the dictionary to be updated + UDEBUG("Joining dictionary update thread... thread finished!"); } std::list wordIds; @@ -4255,10 +4107,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p std::vector imageBytes; std::vector depthBytes; - if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) + if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1) { - UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion."); - depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage); + UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format)."); + depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage); } rtabmap::CompressionThread ctImage(image, std::string(".jpg")); @@ -4284,6 +4136,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), + data.laserScanMaxRange(), ctImage.getCompressedData(), ctDepth.getCompressedData(), stereoCameraModel, @@ -4293,6 +4146,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), + data.laserScanMaxRange(), ctImage.getCompressedData(), ctDepth.getCompressedData(), cameraModels, @@ -4319,6 +4173,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), + data.laserScanMaxRange(), cv::Mat(), cv::Mat(), stereoCameraModel, @@ -4328,12 +4183,13 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p SensorData( ctDepth2d.getCompressedData(), data.laserScanMaxPts(), + data.laserScanMaxRange(), cv::Mat(), cv::Mat(), cameraModels, id, 0, - ctUserData.getCompressedData())); + ctUserData.getCompressedData())); } s->setWords(words); s->setWords3(words3D); @@ -4341,11 +4197,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p { s->sensorData().setImageRaw(image); s->sensorData().setDepthOrRightRaw(depthOrRightImage); - s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts()); + s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts(), data.laserScanMaxRange()); s->sensorData().setUserDataRaw(data.userDataRaw()); } - t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); UDEBUG("time compressing data (id=%d) %fs", id, t); @@ -4596,7 +4451,7 @@ void Memory::getMetricConstraints( const Signature * s2 = this->getSignature(uter->first); if(s2) { - link = link.merge(uter->second); + link = link.merge(uter->second, uter->second.type()); poses.erase(s->id()); s = s2; } diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 51ae1bf9..705819b1 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UConversion.h" -#include "ParticleFilter.h" +#include "rtabmap/core/ParticleFilter.h" namespace rtabmap { @@ -54,6 +54,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _estimationType(Parameters::defaultOdomEstimationType()), _pnpReprojError(Parameters::defaultOdomPnPReprojError()), _pnpFlags(Parameters::defaultOdomPnPFlags()), + _varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()), _resetCurrentCount(0), previousStamp_(0), previousTransform_(Transform::getIdentity()), @@ -73,6 +74,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError); Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags); UASSERT(_pnpFlags>=0 && _pnpFlags <=2); + Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount); Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering); Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize); Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT); @@ -269,6 +271,11 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info) { distanceTravelled_ += t.getNorm(); info->distanceTravelled = distanceTravelled_; + + if(_varianceFromInliersCount) + { + info->variance = info->inliers > 0?1.0/double(info->inliers):1.0; + } } return _pose *= t; // updated diff --git a/corelib/src/OdometryBOW.cpp b/corelib/src/OdometryBOW.cpp index 03678c0a..ab483bb5 100644 --- a/corelib/src/OdometryBOW.cpp +++ b/corelib/src/OdometryBOW.cpp @@ -120,6 +120,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) : // init the local map with a all 3D features contained in the database customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false")); customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); + customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false")); _memory = new Memory(customParameters); if(!_memory->init(_fixedLocalMapPath, false, ParametersMap())) { diff --git a/corelib/src/OdometryICP.cpp b/corelib/src/OdometryICP.cpp index 90af92df..70437332 100644 --- a/corelib/src/OdometryICP.cpp +++ b/corelib/src/OdometryICP.cpp @@ -72,6 +72,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * bool hasConverged = false; double variance = 0; unsigned int minPoints = 100; + int correspondences = 0; if(!data.depthOrRightRaw().empty()) { if(data.depthOrRightRaw().type() == CV_8UC1) @@ -121,7 +122,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * hasConverged, *newCloudRegistered); - int correspondences = 0; util3d::computeVarianceAndCorrespondences( newCloudRegistered, _previousCloudNormal, @@ -164,7 +164,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * hasConverged, *newCloudRegistered); - int correspondences = 0; util3d::computeVarianceAndCorrespondences( newCloudRegistered, _previousCloud, @@ -202,6 +201,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * if(info) { info->variance = variance; + info->inliers = correspondences; } UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d", diff --git a/corelib/src/OdometryMono.cpp b/corelib/src/OdometryMono.cpp index ed877f11..48b57dd5 100644 --- a/corelib/src/OdometryMono.cpp +++ b/corelib/src/OdometryMono.cpp @@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UMath.h" #include #include #include diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 68d1347e..1f900793 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -35,7 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" -#include "BayesFilter.h" +#include "rtabmap/core/BayesFilter.h" +#include "rtabmap/core/Compression.h" #include #include @@ -93,6 +94,7 @@ Rtabmap::Rtabmap() : _poseScanMatching(Parameters::defaultRGBDPoseScanMatching()), _localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()), _localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()), + _scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()), _localRadius(Parameters::defaultRGBDLocalRadius()), _localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()), _localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()), @@ -100,6 +102,7 @@ Rtabmap::Rtabmap() : _localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()), _databasePath(""), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), + _optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()), _reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()), _reextractNNType(Parameters::defaultLccReextractNNType()), _reextractNNDR(Parameters::defaultLccReextractNNDR()), @@ -108,11 +111,15 @@ Rtabmap::Rtabmap() : _reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()), _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()), _goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()), - _planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()), _goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()), + _pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()), + _pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()), + _pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()), _loopClosureHypothesis(0,0.0f), _highestHypothesis(0,0.0f), _lastProcessTime(0.0), + _someNodesHaveBeenTransferred(false), + _distanceTravelled(0.0f), _epipolarGeometry(0), _bayesFilter(0), _graphOptimizer(0), @@ -121,10 +128,12 @@ Rtabmap::Rtabmap() : _foutInt(0), _wDir("."), _mapCorrection(Transform::getIdentity()), - _mapTransform(Transform::getIdentity()), + _lastLocalizationNodeId(0), + _pathStatus(0), _pathCurrentIndex(0), _pathGoalIndex(0), - _pathTransformToGoal(Transform::getIdentity()) + _pathTransformToGoal(Transform::getIdentity()), + _pathStuckCount(0) { } @@ -228,6 +237,8 @@ void Rtabmap::setupLogFiles(bool overwrite) fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n"); fprintf(_foutInt, " 18-Local graph size\n"); fprintf(_foutInt, " 19-Sensor data id\n"); + fprintf(_foutInt, " 20-Indexed words\n"); + fprintf(_foutInt, " 21-Index memory usage (KB)\n"); } ULOGGER_DEBUG("Log file (int)=%s", (_wDir+"/"+LOG_I).c_str()); @@ -319,12 +330,14 @@ void Rtabmap::close() _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; + _someNodesHaveBeenTransferred = false; _optimizedPoses.clear(); _constraints.clear(); _mapCorrection.setIdentity(); - _mapTransform.setIdentity(); _lastLocalizationPose.setNull(); - this->clearPath(); + _lastLocalizationNodeId = 0; + _distanceTravelled = 0.0f; + this->clearPath(0); flushStatisticLogs(); if(_foutFloat) @@ -392,12 +405,14 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace); + Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks); Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius); Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed); Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); + Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError); Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures); Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType); Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR); @@ -406,8 +421,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius); - Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks); Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData); + Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations); + Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity); + Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity); + + UASSERT(_rgbdLinearUpdate >= 0.0f); + UASSERT(_rgbdAngularUpdate >= 0.0f); // RGB-D SLAM stuff if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end()) @@ -642,6 +662,8 @@ int Rtabmap::triggerNewMap() UINFO("New map triggered, new map = %d", mapId); _optimizedPoses.clear(); _constraints.clear(); + _lastLocalizationNodeId = 0; + _distanceTravelled = 0.0f; } return mapId; } @@ -718,7 +740,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin) } } -void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int type) +void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format) { if(_memory && _memory->getLastWorkingSignature()) { @@ -735,70 +757,21 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - if(type==2) // TORO + std::map stamps; + if(format == 1) { - graph::TOROOptimizer::saveGraph(path, poses, constraints); - } - else - { - //get timestamps - std::map stamps; - if(type == 1) + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - Transform o; - int m, w; - std::string l; - double stamp = 0.0; - _memory->getNodeInfo(iter->first, o, m, w, l, stamp, true); - stamps.insert(std::make_pair(iter->first, stamp)); - } - UASSERT(stamps.size()== 0 || stamps.size() == poses.size()); - } - - FILE* fout = 0; -#ifdef _MSC_VER - fopen_s(&fout, path.c_str(), "w"); -#else - fout = fopen(path.c_str(), "w"); -#endif - if(fout) - { - for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - if(type == 1) // rgbd-slam format - { - // Format: stamp x y z qw qx qy qz - Eigen::Quaternionf q = (*iter).second.getQuaternionf(); - - UASSERT(uContains(stamps, iter->first)); - fprintf(fout, "%f %f %f %f %f %f %f %f\n", - stamps.at(iter->first), - (*iter).second.x(), - (*iter).second.y(), - (*iter).second.z(), - q.w(), - q.x(), - q.y(), - q.z()); - } - else // default / KITTI format - { - // Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz - const float * p = (const float *)(*iter).second.data(); - - fprintf(fout, "%f", p[0]); - for(int i=1; i<(*iter).second.size(); i++) - { - fprintf(fout, " %f", p[i]); - } - fprintf(fout, "\n"); - } - } - fclose(fout); + Transform o; + int m, w; + std::string l; + double stamp = 0.0; + _memory->getNodeInfo(iter->first, o, m, w, l, stamp, true); + stamps.insert(std::make_pair(iter->first, stamp)); } } + + graph::exportPoses(path, format, poses, constraints, stamps); } } @@ -807,12 +780,14 @@ void Rtabmap::resetMemory() _highestHypothesis = std::make_pair(0,0.0f); _loopClosureHypothesis = std::make_pair(0,0.0f); _lastProcessTime = 0.0; + _someNodesHaveBeenTransferred = false; _optimizedPoses.clear(); _constraints.clear(); _mapCorrection.setIdentity(); - _mapTransform.setIdentity(); _lastLocalizationPose.setNull(); - this->clearPath(); + _lastLocalizationNodeId = 0; + _distanceTravelled = 0.0f; + this->clearPath(0); if(_memory) { @@ -880,7 +855,6 @@ bool Rtabmap::process( std::map childCount; std::set signaturesRetrieved; int localLoopClosuresInTimeFound = 0; - bool scanMatchingSuccess = false; const Signature * signature = 0; const Signature * sLoop = 0; @@ -975,7 +949,7 @@ bool Rtabmap::process( UFATAL("Not supposed to be here...last signature is null?!?"); } - ULOGGER_INFO("Processing signature %d", signature->id()); + ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight()); timeMemoryUpdate = timer.ticks(); ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate); @@ -991,7 +965,7 @@ bool Rtabmap::process( { _optimizedPoses.erase(rehearsedId); } - else if(_rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f) + else if(signature->getWeight() >= 0 && _rgbdLinearUpdate > 0.0f && _rgbdAngularUpdate > 0.0f) { //============================================================ // Minimum displacement required to add to Memory @@ -999,67 +973,115 @@ bool Rtabmap::process( const std::map & links = signature->getLinks(); if(links.size() == 1) { - float x,y,z, roll,pitch,yaw; - links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if((_rgbdLinearUpdate==0.0f || ( - fabs(x) < _rgbdLinearUpdate && - fabs(y) < _rgbdLinearUpdate && - fabs(z) < _rgbdLinearUpdate)) && - (_rgbdAngularUpdate==0.0f || ( - fabs(roll) < _rgbdAngularUpdate && - fabs(pitch) < _rgbdAngularUpdate && - fabs(yaw) < _rgbdAngularUpdate))) + // don't do this if there are intermediate nodes + const Signature * s = _memory->getSignature(links.begin()->second.to()); + UASSERT(s!=0); + if(s->getWeight() >= 0) { - // This will disable global loop closure detection, only retrieval will be done. - // The location will also be deleted at the end. - smallDisplacement = true; + float x,y,z, roll,pitch,yaw; + links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + bool isMoving = fabs(x) > _rgbdLinearUpdate || + fabs(y) > _rgbdLinearUpdate || + fabs(z) > _rgbdLinearUpdate || + fabs(roll) > _rgbdAngularUpdate || + fabs(pitch) > _rgbdAngularUpdate || + fabs(yaw) > _rgbdAngularUpdate; + if(!isMoving) + { + // This will disable global loop closure detection, only retrieval will be done. + // The location will also be deleted at the end. + smallDisplacement = true; + } } } } - Transform newPose = _mapCorrection * signature->getPose(); - _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); - _lastLocalizationPose = newPose; // used in localization mode only (path planning) - - //============================================================ - // Scan matching - //============================================================ - if(_poseScanMatching && - signature->getLinks().size() == 1 && - !signature->sensorData().laserScanCompressed().empty() && - rehearsedId == 0) // don't do it if rehearsal happened + // Update optimizedPoses with the newly added node + Transform newPose; + if(signature->getLinks().size() == 1 && + !smallDisplacement && + _memory->isIncremental()) // ignore pose matching in localization mode { - UINFO("Odometry correction by scan matching"); int oldId = signature->getLinks().begin()->first; const Signature * oldS = _memory->getSignature(oldId); UASSERT(oldS != 0); - std::string rejectedMsg; - Transform guess = signature->getLinks().begin()->second.transform(); - double variance = 1.0; - int inliers = 0; - float inliersRatio = 0; - Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio); - if(!t.isNull()) + + //============================================================ + // Scan matching + //============================================================ + if(_poseScanMatching && + !signature->sensorData().laserScanCompressed().empty() && + rehearsedId == 0) // don't do it if rehearsal happened { - scanMatchingSuccess = true; - UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s", - signature->id(), - oldId, - signature->getLinks().at(oldId).transform().prettyPrint().c_str(), - t.prettyPrint().c_str()); - _memory->updateLink(signature->id(), oldId, t, variance, variance); + UINFO("Odometry correction by scan matching"); + Transform guess = signature->getLinks().begin()->second.transform(); + double variance = 1.0; + int inliers = 0; + float inliersRatio = 0; + std::string rejectedMsg; + Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio); + if(!t.isNull()) + { + UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s", + signature->id(), + oldId, + variance, + signature->getLinks().at(oldId).transform().prettyPrint().c_str(), + t.prettyPrint().c_str()); + UASSERT(variance > 0.0); + _memory->updateLink(signature->id(), oldId, t, variance, variance); + + if(_optimizeFromGraphEnd) + { + // update all previous nodes + // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd + // parameters just changed state, we should put back all poses without map correction. + Transform u = guess.inverse() * t; + std::map::iterator jter = _optimizedPoses.find(oldId); + UASSERT(jter!=_optimizedPoses.end()); + Transform up = jter->second * u * jter->second.inverse(); + Transform mapCorrectionInv = _mapCorrection.inverse(); + for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) + { + iter->second = mapCorrectionInv * up * iter->second; + } + } + } + else + { + UINFO("Scan matching rejected: %s", rejectedMsg.c_str()); + if(variance > 0) + { + double sqrtVar = sqrt(variance); + _memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar); + } + } + statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0); + statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers); + statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio); + statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance); } - else + timeScanMatching = timer.ticks(); + ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching); + + UASSERT(oldS->hasLink(signature->id())); + UASSERT(uContains(_optimizedPoses, oldId)); + newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform(); + _mapCorrection = newPose * signature->getPose().inverse(); + if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) { - UINFO("Scan matching rejected: %s", rejectedMsg.c_str()); + UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s", + _mapCorrection.prettyPrint().c_str(), + newPose.prettyPrint().c_str(), + signature->getPose().prettyPrint().c_str()); } - statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), scanMatchingSuccess?1.0f:0); - statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers); - statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio); - statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance); } - timeScanMatching = timer.ticks(); - ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching); + else + { + newPose = _mapCorrection * signature->getPose(); + } + _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); + _lastLocalizationPose = newPose; // used in localization mode only (path planning) if(signature->getLinks().size() == 1) { @@ -1069,6 +1091,8 @@ bool Rtabmap::process( Link tmp = signature->getLinks().begin()->second.inverse(); + _distanceTravelled += tmp.transform().getNorm(); + // if the previous node is an intermediate node, remove it from the local graph if(_constraints.size() && _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to()) @@ -1077,7 +1101,7 @@ bool Rtabmap::process( UASSERT(s!=0); if(s->getWeight() == -1) { - tmp = _constraints.rbegin()->second.merge(tmp); + tmp = _constraints.rbegin()->second.merge(tmp, tmp.type()); _optimizedPoses.erase(s->id()); _constraints.erase(--_constraints.end()); } @@ -1090,7 +1114,8 @@ bool Rtabmap::process( //============================================================ if(_localLoopClosureDetectionTime && rehearsedId == 0 && // don't do it if rehearsal happened - signature->getWords3().size()) + signature->getWords3().size() && + _memory->isIncremental()) // don't do it in localization mode { const std::set & stm = _memory->getStMem(); for(std::set::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter) @@ -1107,7 +1132,7 @@ bool Rtabmap::process( if(!transform.isNull() && _globalLoopClosureIcpType > 0) { transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance); - variance = 1.0f; // ICP, set variance to 1 + variance = 1.0f; // ICP, set variance to 1 // FIXME why? all other links based on visual keep the variance } if(!transform.isNull()) { @@ -1116,6 +1141,7 @@ bool Rtabmap::process( *iter, transform.prettyPrint().c_str()); // Add a loop constraint + UASSERT(variance > 0.0); if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance))) { ++localLoopClosuresInTimeFound; @@ -1145,7 +1171,7 @@ bool Rtabmap::process( // Bayes filter update //============================================================ int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0; - // Not a bad signature, not a small displacemnt unless the previous signature didn't have a loop closure + // Not a bad signature, not a small displacement unless the previous signature didn't have a loop closure if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)) { // If the working memory is empty, don't do the detection. It happens when it @@ -1267,6 +1293,7 @@ bool Rtabmap::process( // When analysing logs, it's convenient to know // if the hypothesis would be rejected if T_loop would be lower. rejectedHypothesis = true; + UWARN("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second); } //for statistic... @@ -1516,7 +1543,7 @@ bool Rtabmap::process( if(immunizedLocally >= maxLocalLocationsImmunized) { // set 20 to avoid this warning when starting mapping - if(maxLocalLocationsImmunized > 20) + if(maxLocalLocationsImmunized > 20 && _someNodesHaveBeenTransferred) { UWARN("Could not immunize the whole local path (%d) between " "%d and %d (max location immunized=%d). You may want " @@ -1648,6 +1675,7 @@ bool Rtabmap::process( // Update loop closure links // (updated: place this after retrieval to be sure that neighbors of the loop closure are in RAM) //============================================================= + std::list > loopClosureLinksAdded; int loopClosureVisualInliers = 0; // for statistics if(_loopClosureHypothesis.first>0) { @@ -1723,27 +1751,25 @@ bool Rtabmap::process( rejectedHypothesis = transform.isNull(); if(rejectedHypothesis) { - UINFO("Rejected loop closure %d -> %d: %s", + UWARN("Rejected loop closure %d -> %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str()); } } if(!rejectedHypothesis) { // Make the new one the parent of the old one + UASSERT(variance > 0.0); rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance)); + if(!rejectedHypothesis) + { + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first)); + } } if(rejectedHypothesis) { _loopClosureHypothesis.first = 0; } - else - { - const Signature * oldS = _memory->getSignature(_loopClosureHypothesis.first); - UASSERT(oldS != 0); - // Old map -> new map, used for localization correction on loop closure - _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse(); - } } timeAddLoopClosureLink = timer.ticks(); @@ -1776,41 +1802,45 @@ bool Rtabmap::process( // // 1) compare visually with nearest locations // - float r = _localRadius; - if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius) - { - r = _localPathFilteringRadius; - } - + UDEBUG("Proximity detection (local loop closure in SPACE using matching images)"); std::map nearestIds; if(_memory->isIncremental()) { - nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth); + nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _localDetectMaxGraphDepth); } else { - nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, r); + nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius); } + UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size()); std::map nearestPoses; for(std::map::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter) { - nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + if(_memory->getStMem().find(iter->first) == _memory->getStMem().end()) + { + nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first))); + } } + UDEBUG("nearestPoses=%d", (int)nearestPoses.size()); + // segment poses by paths, only one detection per path std::list > nearestPaths = getPaths(nearestPoses); - for(std::list >::iterator iter=nearestPaths.begin(); + UDEBUG("nearestPaths=%d", (int)nearestPaths.size()); + + for(std::list >::const_iterator iter=nearestPaths.begin(); iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0); ++iter) { - std::map & path = *iter; + const std::map & path = *iter; UASSERT(path.size()); //find the nearest pose on the path int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); UASSERT(nearestId > 0); - // nearest pose must not be linked to current location, and not in STM + // nearest pose must not be linked to current location and enough if(!signature->hasLink(nearestId) && - _memory->getStMem().find(nearestId) == _memory->getStMem().end()) + (_localPathFilteringRadius <= 0.0f || + _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius)) { double variance = 1.0; Transform transform; @@ -1879,20 +1909,27 @@ bool Rtabmap::process( } if(!transform.isNull()) { - UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s", - signature->id(), - nearestId, - transform.prettyPrint().c_str()); - _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); - - if(_loopClosureHypothesis.first == 0) + if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius) { - // Old map -> new map, used for localization correction on loop closure - const Signature * oldS = _memory->getSignature(nearestId); - UASSERT(oldS != 0); - _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse(); - ++localSpaceClosuresAddedVisually; - lastLocalSpaceClosureId = nearestId; + UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s", + signature->id(), + nearestId, + transform.prettyPrint().c_str()); + UASSERT(variance > 0.0); + _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); + + if(_loopClosureHypothesis.first == 0) + { + ++localSpaceClosuresAddedVisually; + lastLocalSpaceClosureId = nearestId; + } + } + else + { + UWARN("Ignoring local loop closure with %d because resulting " + "transform is to large!? (%fm > %fm)", + nearestId, transform.getNorm(), _localPathFilteringRadius); } } } @@ -1901,6 +1938,7 @@ bool Rtabmap::process( // // 2) compare locally with nearest locations by scan matching // + UDEBUG("Proximity detection (local loop closure in SPACE with scan matching)"); if( !signature->sensorData().laserScanCompressed().empty() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0)) { @@ -1908,18 +1946,10 @@ bool Rtabmap::process( // closures if we are already localized by at least one // local visual closure above. - std::map forwardPoses; - forwardPoses = this->getForwardWMPoses( - signature->id(), - 0, - _localRadius, - _localDetectMaxGraphDepth); + localSpacePaths = (int)nearestPaths.size(); - std::list > forwardPaths = getPaths(forwardPoses); - localSpacePaths = (int)forwardPaths.size(); - - for(std::list >::iterator iter=forwardPaths.begin(); - iter!=forwardPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0); + for(std::list >::iterator iter=nearestPaths.begin(); + iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0); ++iter) { std::map & path = *iter; @@ -1928,6 +1958,7 @@ bool Rtabmap::process( //find the nearest pose on the path int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); UASSERT(nearestId > 0); + UDEBUG("Path %d distance=%fm", nearestId, _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); // nearest pose must be close and not linked to current location if(!signature->hasLink(nearestId) && @@ -1940,6 +1971,7 @@ bool Rtabmap::process( //optimize the path's poses locally path = optimizeGraph(nearestId, uKeysSet(path), false); // transform local poses in optimized graph referential + UASSERT(uContains(path, nearestId)); Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse(); for(std::map::iterator jter=path.begin(); jter!=path.end(); ++jter) { @@ -1949,7 +1981,7 @@ bool Rtabmap::process( if(_localPathFilteringRadius > 0.0f) { // path filtering - std::map filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true); + std::map filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true); // make sure the nearest and farthest poses are still here filteredPath.insert(*path.find(nearestId)); filteredPath.insert(*path.begin()); @@ -1964,31 +1996,66 @@ bool Rtabmap::process( //The nearest will be the reference for a loop closure transform if(signature->getLinks().find(nearestId) == signature->getLinks().end()) { - Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0); + double variance = 1.0; + Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance); if(!transform.isNull()) { - UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s", - signature->id(), - nearestId, - transform.prettyPrint().c_str()); - // set Identify covariance for laser scan matching only - _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, 1, 1)); - - ++localSpaceClosuresAddedByICPOnly; - - // no local loop closure added visually - if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0) + if(_localPathFilteringRadius <= 0 || transform.getNormSquared() <= _localPathFilteringRadius*_localPathFilteringRadius) { - // Old map -> new map, used for localization correction on loop closure - const Signature * oldS = _memory->getSignature(nearestId); - UASSERT(oldS != 0); - _mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse(); - lastLocalSpaceClosureId = nearestId; + UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s", + signature->id(), + nearestId, + transform.prettyPrint().c_str()); + + cv::Mat scanMatchingIds; + if(_scanMatchingIdsSavedInLinks) + { + std::stringstream stream; + stream << "SCANS:"; + for(std::map::iterator iter=path.begin(); iter!=path.end(); ++iter) + { + if(iter->first!=signature->id()) + { + if(iter != path.begin()) + { + stream << ";"; + } + stream << uNumber2Str(iter->first); + } + } + std::string scansStr = stream.str(); + scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str()); + scanMatchingIds = compressData2(scanMatchingIds); // compressed + } + + // set Identify covariance for laser scan matching only + UASSERT(variance>0.0); + double sqrtVar = sqrt(variance); + _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds)); + loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); + + ++localSpaceClosuresAddedByICPOnly; + + // no local loop closure added visually + if(localSpaceClosuresAddedVisually == 0 && _loopClosureHypothesis.first == 0) + { + lastLocalSpaceClosureId = nearestId; + } + } + else + { + UWARN("Ignoring local loop closure with %d because resulting " + "transform is to large!? (%fm > %fm)", + nearestId, transform.getNorm(), _localPathFilteringRadius); } } } } } + else + { + UDEBUG("Path %d ignored", nearestId); + } } } } @@ -1997,57 +2064,6 @@ bool Rtabmap::process( timeLocalSpaceDetection = timer.ticks(); ULOGGER_INFO("timeLocalSpaceDetection=%fs", timeLocalSpaceDetection); - //============================================================ - // Optimize map graph - //============================================================ - if(_rgbdSlamMode && - (_loopClosureHypothesis.first>0 || // can be different map of the current one - localLoopClosuresInTimeFound>0 || // only same map of the current one - scanMatchingSuccess || // only same map of the current one - lastLocalSpaceClosureId>0 || // can be different map of the current one - signaturesRetrieved.size())) // can be different map of the current one - { - if(_memory->isIncremental()) - { - UINFO("Update map correction: SLAM mode"); - // SLAM mode! - optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints); - UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()); - - // Update map correction, it should be identify when optimizing from the last node - _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); - _mapTransform.setIdentity(); // reset mapTransform (used for localization only) - _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update in case we switch to localization mode - if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) - { - UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); - } - } - else if(_loopClosureHypothesis.first > 0 || lastLocalSpaceClosureId > 0 || signaturesRetrieved.size()) - { - UINFO("Update map correction: Localization mode"); - int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first; - UASSERT(oldId != 0); - if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end()) - { - // update optimized poses - optimizeCurrentMap(oldId, false, _optimizedPoses, &_constraints); - } - UASSERT(_optimizedPoses.find(oldId) != _optimizedPoses.end()); - - // Localization mode! only update map correction - const Signature * oldS = _memory->getSignature(oldId); - UASSERT(oldS != 0); - Transform correction = _optimizedPoses.at(oldId) * oldS->getPose().inverse(); - _mapCorrection = correction * _mapTransform; - _lastLocalizationPose = _mapCorrection * signature->getPose(); - } - else - { - UERROR("Not supposed to be here!"); - } - } - //============================================================ // Add virtual links if a path is activated //============================================================ @@ -2055,17 +2071,152 @@ bool Rtabmap::process( { // Add a virtual loop closure link to keep the path linked to local map if( signature->id() != _path[_pathCurrentIndex].first && - !signature->hasLink(_path[_pathCurrentIndex].first) && - uContains(_optimizedPoses, _path[_pathCurrentIndex].first)) + !signature->hasLink(_path[_pathCurrentIndex].first)) { + UASSERT(uContains(_optimizedPoses, signature->id())); + UASSERT_MSG(uContains(_optimizedPoses, _path[_pathCurrentIndex].first), uFormat("id=%d", _path[_pathCurrentIndex].first).c_str()); Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first); - if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius) + + if(_localRadius == 0.0f || virtualLoop.getNorm() < _localRadius) { _memory->addLink(Link(signature->id(), _path[_pathCurrentIndex].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // set high variance } + else + { + UERROR("Virtual link larger than local radius (%fm > %fm). Aborting the plan!", + virtualLoop.getNorm(), _localRadius); + this->clearPath(-1); + } } } + //============================================================ + // Optimize map graph + //============================================================ + float maxLinearError = 0.0f; + if(_rgbdSlamMode && + (_loopClosureHypothesis.first>0 || + lastLocalSpaceClosureId>0 || // can be different map of the current one + ((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked + (localLoopClosuresInTimeFound>0 || // only same map of the current one + signaturesRetrieved.size())))) // can be different map of the current one + { + UASSERT(uContains(_optimizedPoses, signature->id())); + + // Note that in localization mode, we don't re-optimize the graph + // if: + // 1- there are no signatures retrieved, + // 2- we are relocalizing on a node already in the optimized graph + if(!_memory->isIncremental() && + signaturesRetrieved.size() == 0 && + signature->getLinks().size() && + uContains(_optimizedPoses, signature->getLinks().begin()->first)) + { + // If there are no signatures retrieved, we don't + // need to re-optimize the graph. Just update the last + // position if OptimizeFromGraphEnd=false or transform the + // whole graph if OptimizeFromGraphEnd=true + UINFO("Localization without map optimization"); + if(_optimizeFromGraphEnd) + { + // update all previous nodes + // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd + // parameters just changed state, we should put back all poses without map correction. + Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first); + Transform u = signature->getPose() * signature->getLinks().begin()->second.transform(); + Transform up = u * oldPose.inverse(); + Transform mapCorrectionInv = _mapCorrection.inverse(); + for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) + { + iter->second = mapCorrectionInv * up * iter->second; + } + _optimizedPoses.at(signature->id()) = signature->getPose(); + } + else + { + _optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse(); + } + } + else + { + + UINFO("Update map correction"); + std::map poses = _optimizedPoses; + std::multimap constraints; + optimizeCurrentMap(signature->id(), false, poses, &constraints); + UASSERT(poses.find(signature->id()) != poses.end()); + + // Check added loop closures have broken the graph + // (in case of wrong loop closures). + bool updateConstraints = true; + if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode + _optimizationMaxLinearError > 0.0f && + loopClosureLinksAdded.size()) + { + const Link * maxLinearLink = 0; + for(std::multimap::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) + { + // ignore links with high variance + if(iter->second.transVariance() < 1.0) + { + Transform t1 = uValue(poses, iter->second.from(), Transform()); + Transform t2 = uValue(poses, iter->second.to(), Transform()); + Transform t = t1.inverse()*t2; + float linearError = uMax3( + fabs(iter->second.transform().x() - t.x()), + fabs(iter->second.transform().y() - t.y()), + fabs(iter->second.transform().z() - t.z())); + if(linearError > maxLinearError) + { + maxLinearError = linearError; + maxLinearLink = &iter->second; + } + } + } + + if(maxLinearError > _optimizationMaxLinearError) + { + UWARN("Rejecting all added loop closures (%d) in this " + "iteration because a wrong loop closure has been " + "detected after graph optimization, resulting in " + "a maximum graph error of %f m (edge %d->%d, type=%d). The " + "maximum error parameter is %f m.", + (int)loopClosureLinksAdded.size(), + maxLinearError, + maxLinearLink->from(), + maxLinearLink->to(), + maxLinearLink->type(), + _optimizationMaxLinearError); + for(std::list >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter) + { + _memory->removeLink(iter->first, iter->second); + UWARN("Loop closure %d->%d rejected!", iter->first, iter->second); + } + updateConstraints = false; + _loopClosureHypothesis.first = 0; + lastLocalSpaceClosureId = 0; + rejectedHypothesis = true; + } + } + + if(updateConstraints) + { + UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size()); + _optimizedPoses = poses; + _constraints = constraints; + } + } + + // Update map correction, it should be identify when optimizing from the last node + _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); + _lastLocalizationPose = _optimizedPoses.at(signature->id()); // update in case we switch to localization mode + if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) + { + UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); + } + } + _lastLocalizationNodeId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId>0?lastLocalSpaceClosureId:_lastLocalizationNodeId; + timeMapOptimization = timer.ticks(); ULOGGER_INFO("timeMapOptimization=%fs", timeMapOptimization); @@ -2095,7 +2246,7 @@ bool Rtabmap::process( if(_loopClosureHypothesis.first || _publishStats) { ULOGGER_INFO("sending stats..."); - statistics_.setRefImageId(signature->id()); + statistics_.setRefImageId(_memory->getLastSignatureId()); // Use last id from Memory (in case of rehearsal) if(_loopClosureHypothesis.first != Memory::kIdInvalid) { statistics_.setLoopClosureId(_loopClosureHypothesis.first); @@ -2111,10 +2262,11 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second); statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated); statistics_.addStatistic(Statistics::kLoopVp_hypothesis(), vpHypothesis); - statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId); + statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId); statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio); - statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers); + statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers); statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId()); + statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError); statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound); statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually); @@ -2150,10 +2302,15 @@ bool Rtabmap::process( // Surf specific parameters statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize); + statistics_.addStatistic(Statistics::kKeypointIndexed_words(), _memory->getVWDictionary()->getIndexedWordsCount()); + statistics_.addStatistic(Statistics::kKeypointIndex_memory_usage(), _memory->getVWDictionary()->getIndexMemoryUsed()); //Epipolar geometry constraint statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0); + statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0); + statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled); + if(_publishLikelihood || _publishPdf) { // Child count by parent signature on the root of the memory ... for statistics @@ -2224,9 +2381,9 @@ bool Rtabmap::process( // Pass this point signature should not be used, since it could have been transferred... signature = 0; - + timeMemoryCleanup = timer.ticks(); - ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); + ULOGGER_INFO("timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (int)signaturesRemoved.size()); @@ -2244,8 +2401,13 @@ bool Rtabmap::process( (_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)) { ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed); + immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory std::list transferred = _memory->forget(immunizedLocations); signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end()); + if(!_someNodesHaveBeenTransferred && transferred.size()) + { + _someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization + } } _lastProcessTime = totalTime; @@ -2253,13 +2415,40 @@ bool Rtabmap::process( if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size())) { //refresh the local map because some transferred nodes may have broken the tree - if(_memory->getLastWorkingSignature()) + int id = 0; + if(!_memory->isIncremental() && (_lastLocalizationNodeId > 0 || _path.size())) { - std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true); + if(_path.size()) + { + // priority on node on the path + UASSERT(_pathCurrentIndex < _path.size()); + UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("id=%d", _path.at(_pathCurrentIndex).first).c_str()); + id = _path.at(_pathCurrentIndex).first; + UDEBUG("Refresh local map from %d", id); + } + else + { + UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d", _lastLocalizationNodeId).c_str()); + id = _lastLocalizationNodeId; + UDEBUG("Refresh local map from %d", id); + } + } + else if(_memory->isIncremental() && + _optimizedPoses.size() && + _memory->getLastWorkingSignature()) + { + id = _memory->getLastWorkingSignature()->id(); + UDEBUG("Refresh local map from %d", id); + } + if(id > 0) + { + UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str()); + std::map ids = _memory->getNeighborsId(id, 0, 0, true); for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();) { if(!uContains(ids, iter->first)) { + UDEBUG("Removed %d from local map", iter->first); _optimizedPoses.erase(iter++); } else @@ -2285,6 +2474,14 @@ bool Rtabmap::process( _constraints.clear(); } } + // just some verifications to make sure that planning path is still in the local map! + if(_path.size()) + { + UASSERT(_pathCurrentIndex < _path.size()); + UASSERT(_pathGoalIndex < _path.size()); + UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathCurrentIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathCurrentIndex).first).c_str()); + UASSERT_MSG(uContains(_optimizedPoses, _path.at(_pathGoalIndex).first), uFormat("local map size=%d, id=%d", (int)_optimizedPoses.size(), _path.at(_pathGoalIndex).first).c_str()); + } timeRealTimeLimitReachedProcess = timer.ticks(); @@ -2390,7 +2587,7 @@ bool Rtabmap::process( timeLocalTimeDetection, timeLocalSpaceDetection, timeMapOptimization); - std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", + std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", _loopClosureHypothesis.first, _highestHypothesis.first, (int)signaturesRemoved.size(), @@ -2409,7 +2606,9 @@ bool Rtabmap::process( rehearsalMaxId, rehearsalMaxId>0?1:0, localGraphSize, - data.id()); + data.id(), + _memory->getVWDictionary()->getIndexedWordsCount(), + _memory->getVWDictionary()->getIndexMemoryUsed()); if(_statisticLogsBufferedInRAM) { _bufferedLogsF.push_back(logF); @@ -2624,7 +2823,6 @@ std::map Rtabmap::getForwardWMPoses( return poses; } -// Get paths in front of the robot, returned optimized poses std::list > Rtabmap::getPaths(std::map poses) const { std::list > paths; @@ -2661,7 +2859,6 @@ void Rtabmap::optimizeCurrentMap( std::multimap * constraints) const { //Optimize the map - optimizedPoses.clear(); UINFO("Optimize map: around location %d", id); if(_memory && id > 0) { @@ -2673,14 +2870,23 @@ void Rtabmap::optimizeCurrentMap( } UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks()); - optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); - - if(_memory->getSignature(id) && uContains(optimizedPoses, id)) - { - Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); - UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); - } + std::map poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); UINFO("optimize time %f s", timer.ticks()); + + if(poses.size()) + { + optimizedPoses = poses; + + if(_memory->getSignature(id) && uContains(optimizedPoses, id)) + { + Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); + UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); + } + } + else + { + UERROR("Failed to optimize the graph! Keeping the graph without optimization..."); + } } } @@ -2922,8 +3128,8 @@ void Rtabmap::getGraph( std::map & poses, std::multimap & constraints, bool optimized, - bool global, - std::map * signatures) + bool global, + std::map * signatures) { if(_memory && _memory->getLastWorkingSignature()) { @@ -2945,8 +3151,8 @@ void Rtabmap::getGraph( std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true); _memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global); } - - if(signatures) + + if(signatures) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { @@ -2962,7 +3168,7 @@ void Rtabmap::getGraph( weight, stamp, label, - odomPose))); + odomPose))); } } } @@ -2976,12 +3182,16 @@ void Rtabmap::getGraph( } } -void Rtabmap::clearPath() +void Rtabmap::clearPath(int status) { + UINFO("status=%d", status); + _pathStatus = status; _path.clear(); _pathCurrentIndex=0; _pathGoalIndex = 0; _pathTransformToGoal.setIdentity(); + _pathUnreachableNodes.clear(); + _pathStuckCount = 0; if(_memory) { _memory->removeAllVirtualLinks(); @@ -2992,7 +3202,7 @@ void Rtabmap::clearPath() bool Rtabmap::computePath(int targetNode, bool global) { UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0); - this->clearPath(); + this->clearPath(0); if(!_rgbdSlamMode) { @@ -3031,7 +3241,10 @@ bool Rtabmap::computePath(int targetNode, bool global) currentNode, targetNode, _memory, - global); + global, + false, + _pathLinearVelocity, + _pathAngularVelocity); //transform in current referential Transform t = uValue(_optimizedPoses, currentNode, Transform::getIdentity()); @@ -3050,6 +3263,7 @@ bool Rtabmap::computePath(int targetNode, bool global) { _path.clear(); UWARN("Cannot compute a path!"); + return false; } else { @@ -3074,16 +3288,17 @@ bool Rtabmap::computePath(int targetNode, bool global) setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); } updateGoalIndex(); + return _path.size() || _pathStatus > 0; } - return _path.size()>0; + return false; } bool Rtabmap::computePath(const Transform & targetPose) { UINFO("Planning a path to pose %s ", targetPose.prettyPrint().c_str()); - this->clearPath(); + this->clearPath(0); std::list > pathPoses; if(!_rgbdSlamMode) @@ -3102,10 +3317,14 @@ bool Rtabmap::computePath(const Transform & targetPose) UASSERT(s); for(std::map::const_iterator jter=s->getLinks().begin(); jter!=s->getLinks().end(); ++jter) { - links.insert(std::make_pair(jter->second.from(), jter->second.to())); - links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> + // only add links for which poses are in "nodes" + if(uContains(nodes, jter->second.to())) + { + links.insert(std::make_pair(jter->second.from(), jter->second.to())); + //links.insert(std::make_pair(jter->second.to(), jter->second.from())); // <-> (commented: already added when iterating in nodes) + } } - } + } UINFO("Time getting links = %fs", timer.ticks()); int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose); @@ -3139,20 +3358,6 @@ bool Rtabmap::computePath(const Transform & targetPose) currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); } - // Add links between neighbor nodes in the goal radius. - if(_planVirtualLinks) - { - std::multimap clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI); - for(std::multimap::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) - { - if(graph::findLink(links, iter->first, iter->second) == links.end()) - { - links.insert(*iter); - links.insert(std::make_pair(iter->second, iter->first)); // <-> - } - } - } - UINFO("Computing path from location %d to %d", currentNode, nearestId); UTimer timer; _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId)); @@ -3160,7 +3365,6 @@ bool Rtabmap::computePath(const Transform & targetPose) if(_path.size() == 0) { - _path.clear(); UWARN("Cannot compute a path!"); } else @@ -3184,6 +3388,8 @@ bool Rtabmap::computePath(const Transform & targetPose) _pathTransformToGoal = nodes.at(_path.back().first).inverse() * targetPose; updateGoalIndex(); + + return true; } } } @@ -3192,7 +3398,7 @@ bool Rtabmap::computePath(const Transform & targetPose) UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str()); } - return _path.size()>0; + return false; } std::vector > Rtabmap::getPathNextPoses() const @@ -3274,6 +3480,8 @@ void Rtabmap::updateGoalIndex() } } // for the current index, only keep the newest virtual link + // This will make sure that the path is still connected even + // if the new signature is removed (e.g., because of a small displacement) UASSERT(_pathCurrentIndex < _path.size()); const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first); UASSERT(currentIndexS != 0); @@ -3296,7 +3504,7 @@ void Rtabmap::updateGoalIndex() // Make sure the next signatures on the path are linked together float distanceSoFar = 0.0f; - for(unsigned int i=_pathCurrentIndex; + for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i) { @@ -3314,7 +3522,7 @@ void Rtabmap::updateGoalIndex() if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) { Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; - _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 1, 1)); // on the optimized path, set Identity variance + _memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); } } @@ -3334,7 +3542,7 @@ void Rtabmap::updateGoalIndex() !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id())) { UERROR("Last node is null in memory or not in optimized poses. Aborting the plan..."); - this->clearPath(); + this->clearPath(-1); return; } currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()); @@ -3344,7 +3552,7 @@ void Rtabmap::updateGoalIndex() if(_lastLocalizationPose.isNull()) { UERROR("Last localization pose is null. Aborting the plan..."); - this->clearPath(); + this->clearPath(-1); return; } currentPose = _lastLocalizationPose; @@ -3358,31 +3566,36 @@ void Rtabmap::updateGoalIndex() if(d < _goalReachedRadius) { UINFO("Goal %d reached!", goalId); - this->clearPath(); + this->clearPath(1); } } if(_path.size()) { //Always check if the farthest node is accessible in local map (max to local space radius if set) - int goalIndex = _pathCurrentIndex; + unsigned int goalIndex = _pathCurrentIndex; float distanceFromCurrentNode = 0.0f; - for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i) + bool sameGoalIndex = false; + for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i) { if(uContains(_optimizedPoses, _path[i].first)) { if(_localRadius > 0.0f) { - distanceFromCurrentNode = currentPose.getDistance(_optimizedPoses.at(_path[i].first)); + distanceFromCurrentNode += _path[i-1].second.getDistance(_path[i].second); } - if(distanceFromCurrentNode <= _localRadius) + if((goalIndex == _pathCurrentIndex && i == _path.size()-1) || + _pathUnreachableNodes.find(i) == _pathUnreachableNodes.end()) { - goalIndex = i; - } - else - { - break; + if(distanceFromCurrentNode <= _localRadius) + { + goalIndex = i; + } + else + { + break; + } } } else @@ -3390,17 +3603,22 @@ void Rtabmap::updateGoalIndex() break; } } - UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size()); - if((int)_pathGoalIndex != goalIndex) + UASSERT(_pathGoalIndex < _path.size() && goalIndex < _path.size()); + if(_pathGoalIndex != goalIndex) { UINFO("Updated current goal from %d to %d (%d/%d)", - (int)_path[_pathGoalIndex].first, _path[goalIndex].first, goalIndex+1, (int)_path.size()); + (int)_path[_pathGoalIndex].first, _path[goalIndex].first, (int)goalIndex+1, (int)_path.size()); _pathGoalIndex = goalIndex; } + else + { + sameGoalIndex = true; + } // update nearest pose in the path unsigned int nearestNodeIndex = 0; float distance = -1.0f; + bool sameCurrentIndex = false; UASSERT(_pathGoalIndex < _path.size() && _pathGoalIndex >= 0); for(unsigned int i=_pathCurrentIndex; i<=_pathGoalIndex; ++i) { @@ -3418,7 +3636,7 @@ void Rtabmap::updateGoalIndex() if(distance < 0) { UERROR("The nearest pose on the path not found! Aborting the plan..."); - this->clearPath(); + this->clearPath(-1); } else { @@ -3427,6 +3645,38 @@ void Rtabmap::updateGoalIndex() if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex) { _pathCurrentIndex = nearestNodeIndex; + _pathUnreachableNodes.erase(nearestNodeIndex); // if we are on it, it is reachable + } + else + { + sameCurrentIndex = true; + } + + if(sameGoalIndex && sameCurrentIndex && + _pathStuckIterations > 0 && + ++_pathStuckCount > _pathStuckIterations) + { + UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.", + _path[_pathGoalIndex].first, + _pathStuckCount, + _pathStuckIterations); + _pathStuckCount = 0; + _pathUnreachableNodes.insert(_pathGoalIndex); + // select previous reachable one + while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end()) + { + if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex) + { + // plan failed! + UERROR("No upcoming nodes on the path are reachable! Aborting the plan..."); + this->clearPath(-1); + return; + } + } + } + else if(!sameGoalIndex || !sameCurrentIndex) + { + _pathStuckCount = 0; } } } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 84d4a0bb..f4e3c522 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -262,10 +262,10 @@ void RtabmapThread::mainLoop() { UERROR("Failed to set a goal to location=%d.", id); } - this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath())); + this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath())); break; case kStateCancellingGoal: - _rtabmap->clearPath(); + _rtabmap->clearPath(0); break; case kStateLabelling: if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str())) @@ -463,12 +463,19 @@ void RtabmapThread::process() { if(_rtabmap->getMemory()) { + bool wasPlanning = _rtabmap->getPath().size()>0; if(_rtabmap->process(data.data(), data.pose(), data.covariance())) { Statistics stats = _rtabmap->getStatistics(); stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_dataBuffer.size()); ULOGGER_DEBUG("posting statistics_ event..."); this->post(new RtabmapEvent(stats)); + + if(wasPlanning && _rtabmap->getPath().size() == 0) + { + // Goal reached or failed + this->post(new RtabmapGoalStatusEvent(_rtabmap->getPathStatus())); + } } } else @@ -510,6 +517,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) lastPose_ = odomEvent.pose(); double maxRotVar = odomEvent.rotVariance(); double maxTransVar = odomEvent.transVariance(); + // FIXME: should merge the transformations/variances like Link::merge(); if(maxRotVar > _rotVariance) { _rotVariance = maxRotVar; diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 6c01cbbe..83b56872 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -38,7 +38,8 @@ namespace rtabmap SensorData::SensorData() : _id(0), _stamp(0.0), - _laserScanMaxPts(0) + _laserScanMaxPts(0), + _laserScanMaxRange(0.0f) { } @@ -50,7 +51,8 @@ SensorData::SensorData( const cv::Mat & userData) : _id(id), _stamp(stamp), - _laserScanMaxPts(0) + _laserScanMaxPts(0), + _laserScanMaxRange(0.0f) { if(image.rows == 1) { @@ -84,6 +86,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(0), + _laserScanMaxRange(0.0f), _cameraModels(std::vector(1, cameraModel)) { if(image.rows == 1) @@ -119,6 +122,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(0), + _laserScanMaxRange(0.0f), _cameraModels(std::vector(1, cameraModel)) { if(rgb.rows == 1) @@ -159,6 +163,7 @@ SensorData::SensorData( SensorData::SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & cameraModel, @@ -168,6 +173,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), + _laserScanMaxRange(laserScanMaxRange), _cameraModels(std::vector(1, cameraModel)) { if(rgb.rows == 1) @@ -224,6 +230,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(0), + _laserScanMaxRange(0.0f), _cameraModels(cameraModels) { if(rgb.rows == 1) @@ -263,6 +270,7 @@ SensorData::SensorData( SensorData::SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & rgb, const cv::Mat & depth, const std::vector & cameraModels, @@ -272,6 +280,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), + _laserScanMaxRange(laserScanMaxRange), _cameraModels(cameraModels) { if(rgb.rows == 1) @@ -328,6 +337,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(0), + _laserScanMaxRange(0.0f), _stereoCameraModel(cameraModel) { if(left.rows == 1) @@ -367,6 +377,7 @@ SensorData::SensorData( SensorData::SensorData( const cv::Mat & laserScan, int laserScanMaxPts, + float laserScanMaxRange, const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & cameraModel, @@ -376,6 +387,7 @@ SensorData::SensorData( _id(id), _stamp(stamp), _laserScanMaxPts(laserScanMaxPts), + _laserScanMaxRange(laserScanMaxRange), _stereoCameraModel(cameraModel) { if(left.rows == 1) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index bd5909df..93983165 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -39,6 +39,7 @@ namespace rtabmap Signature::Signature() : _id(0), // invalid id _mapId(-1), + _stamp(0.0), _weight(0), _saved(false), _modified(true), diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index cb025873..7678e69b 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -66,6 +66,12 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y *this = fromEigen3f(t); } +Transform::Transform(float x, float y, float theta) +{ + Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta); + *this = fromEigen3f(t); +} + bool Transform::isNull() const { return (data()[0] == 0.0f && diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index fd9fee37..bf630610 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/VWDictionary.h" -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/core/Signature.h" #include "rtabmap/core/DBDriver.h" @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UtiLite.h" #include + #if CV_MAJOR_VERSION < 3 #include #else @@ -44,23 +45,254 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #endif #endif +#include "rtflann/flann.hpp" + #include #include namespace rtabmap { +class FlannIndex +{ +public: + FlannIndex(): + index_(0), + nextIndex_(0), + featuresType_(0), + featuresDim_(0), + isLSH_(false) + { + } + virtual ~FlannIndex() + { + this->release(); + } + + void release() + { + if(index_) + { + if(featuresType_ == CV_8UC1) + { + delete (rtflann::Index >*)index_; + } + else + { + delete (rtflann::Index >*)index_; + } + index_ = 0; + } + nextIndex_ = 0; + isLSH_ = false; + addedDescriptors_.clear(); + removedIndexes_.clear(); + } + + unsigned int indexedFeatures() const + { + if(!index_) + { + return 0; + } + if(featuresType_ == CV_8UC1) + { + return ((const rtflann::Index >*)index_)->size(); + } + else + { + return ((const rtflann::Index >*)index_)->size(); + } + } + + // return KB + unsigned int memoryUsed() const + { + if(!index_) + { + return 0; + } + if(featuresType_ == CV_8UC1) + { + return ((const rtflann::Index >*)index_)->usedMemory()/1000; + } + else + { + return ((const rtflann::Index >*)index_)->usedMemory()/1000; + } + } + + void build( + const cv::Mat & features, + const rtflann::IndexParams& params) + { + this->release(); + UASSERT(index_ == 0); + UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1); + featuresType_ = features.type(); + featuresDim_ = features.cols; + + if(featuresType_ == CV_8UC1) + { + rtflann::Matrix dataset(features.data, features.rows, features.cols); + index_ = new rtflann::Index >(dataset, params); + ((rtflann::Index >*)index_)->buildIndex(); + } + else + { + rtflann::Matrix dataset((float*)features.data, features.rows, features.cols); + index_ = new rtflann::Index >(dataset, params); + ((rtflann::Index >*)index_)->buildIndex(); + } + + if(features.rows == 1) + { + // incremental FLANN + addedDescriptors_.insert(std::make_pair(nextIndex_, features)); + } + // else assume that the features are kept in memory outside this class (e.g., dataTree_) + + nextIndex_ = features.rows; + } + + bool isBuilt() + { + return index_!=0; + } + + int featuresType() const {return featuresType_;} + int featuresDim() const {return featuresDim_;} + + unsigned int addPoint(const cv::Mat & feature) + { + if(!index_) + { + UERROR("Flann index not yet created!"); + return 0; + } + UASSERT(feature.type() == featuresType_); + UASSERT(feature.cols == featuresDim_); + UASSERT(feature.rows == 1); + if(featuresType_ == CV_8UC1) + { + rtflann::Matrix point(feature.data, feature.rows, feature.cols); + rtflann::Index > * index = (rtflann::Index >*)index_; + index->addPoints(point, 0); + // Rebuild index if it doubles in size + if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) + { + // clean not used features + for(std::list::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter) + { + addedDescriptors_.erase(*iter); + } + removedIndexes_.clear(); + index->buildIndex(); + } + } + else + { + rtflann::Matrix point((float*)feature.data, feature.rows, feature.cols); + rtflann::Index > * index = (rtflann::Index >*)index_; + index->addPoints(point, 0); + // Rebuild index if it doubles in size + if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) + { + // clean not used features + for(std::list::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter) + { + addedDescriptors_.erase(*iter); + } + removedIndexes_.clear(); + index->buildIndex(); + } + } + + addedDescriptors_.insert(std::make_pair(nextIndex_, feature)); + + return nextIndex_++; + } + + void removePoint(unsigned int index) + { + if(!index_) + { + UERROR("Flann index not yet created!"); + return; + } + + // If a Segmentation fault occurs in removePoint(), verify that you have this fix in your installed "flann/algorithms/nn_index.h": + // 707 - if (ids_[id]==id) { + // 707 + if (id < ids_.size() && ids_[id]==id) { + // ref: https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3#diff-33762b7383f957c2df17301639af5151 + + if(featuresType_ == CV_8UC1) + { + ((rtflann::Index >*)index_)->removePoint(index); + } + else + { + ((rtflann::Index >*)index_)->removePoint(index); + } + removedIndexes_.push_back(index); + } + + void knnSearch( + const cv::Mat & query, + cv::Mat & indices, + cv::Mat & dists, + int knn, + const rtflann::SearchParams& params=rtflann::SearchParams()) + { + if(!index_) + { + UERROR("Flann index not yet created!"); + return; + } + indices.create(query.rows, knn, CV_32S); + dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F); + + rtflann::Matrix indicesF((int*)indices.data, indices.rows, indices.cols); + + if(featuresType_ == CV_8UC1) + { + rtflann::Matrix distsF((unsigned int*)dists.data, dists.rows, dists.cols); + rtflann::Matrix queryF(query.data, query.rows, query.cols); + ((rtflann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } + else + { + rtflann::Matrix distsF((float*)dists.data, dists.rows, dists.cols); + rtflann::Matrix queryF((float*)query.data, query.rows, query.cols); + ((rtflann::Index >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params); + } + } + +private: + void * index_; + unsigned int nextIndex_; + int featuresType_; + int featuresDim_; + bool isLSH_; + + // keep feature in memory until the tree is rebuilt + // (in case the word is deleted when removed from the VWDictionary) + std::map addedDescriptors_; + std::list removedIndexes_; +}; + const int VWDictionary::ID_START = 1; const int VWDictionary::ID_INVALID = 0; VWDictionary::VWDictionary(const ParametersMap & parameters) : _totalActiveReferences(0), _incrementalDictionary(Parameters::defaultKpIncrementalDictionary()), + _incrementalFlann(Parameters::defaultKpIncrementalFlann()), _nndrRatio(Parameters::defaultKpNndrRatio()), _dictionaryPath(Parameters::defaultKpDictionaryPath()), _newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()), _lastWordId(0), - _flannIndex(new cv::flann::Index()), + _flannIndex(new FlannIndex()), _strategy(kNNBruteForce) { this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy()); @@ -78,6 +310,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) ParametersMap::const_iterator iter; Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio); Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether); + Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann); UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str()); @@ -257,7 +490,15 @@ void VWDictionary::setNNStrategy(NNStrategy strategy) } else { + bool update = _strategy != strategy; _strategy = strategy; + if(update) + { + _dataTree = cv::Mat(); + _notIndexedWords = uKeysSet(_visualWords); + _removedIndexedWords.clear(); + this->update(); + } } } } @@ -274,6 +515,16 @@ int VWDictionary::getLastIndexedWordId() const } } +unsigned int VWDictionary::getIndexedWordsCount() const +{ + return _flannIndex->indexedFeatures(); +} + +unsigned int VWDictionary::getIndexMemoryUsed() const +{ + return _flannIndex->memoryUsed(); +} + void VWDictionary::update() { ULOGGER_DEBUG(""); @@ -287,58 +538,144 @@ void VWDictionary::update() if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size()) { - _mapIndexId.clear(); - int oldSize = _dataTree.rows; - _dataTree = cv::Mat(); - _flannIndex->release(); - - if(_visualWords.size()) + if(_incrementalFlann && + _strategy < kNNBruteForce && + _visualWords.size()) { - UTimer timer; - timer.start(); - - int type = _visualWords.begin()->second->getDescriptor().type(); - int dim = _visualWords.begin()->second->getDescriptor().cols; - - UASSERT(type == CV_32F || type == CV_8U); - UASSERT(dim > 0); - - // Create the data matrix - _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F - std::map::const_iterator iter = _visualWords.begin(); - for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size()); + for(std::set::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) { - UASSERT(iter->second->getDescriptor().cols == dim); - UASSERT(iter->second->getDescriptor().type() == type); - - iter->second->getDescriptor().copyTo(_dataTree.row(i)); - _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); + UASSERT(uContains(_mapIdIndex, *iter)); + UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter))); + _flannIndex->removePoint(_mapIdIndex.at(*iter)); + _mapIndexId.erase(_mapIdIndex.at(*iter)); + _mapIdIndex.erase(*iter); } + ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size()); - ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim); - ULOGGER_DEBUG("copying data = %f s", timer.ticks()); - - switch(_strategy) + if(_notIndexedWords.size()) { - case kNNFlannNaive: - _flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); - break; - case kNNFlannKdTree: - UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2); - break; - case kNNFlannLSH: - UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING); - break; - default: - break; + ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size()); + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + { + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + UASSERT(w); + int index = 0; + if(!_flannIndex->isBuilt()) + { + UDEBUG("Building FLANN index..."); + switch(_strategy) + { + case kNNFlannNaive: + _flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams()); + break; + case kNNFlannKdTree: + UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); + _flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams()); + break; + case kNNFlannLSH: + UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); + _flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2)); + break; + default: + UFATAL("Not supposed to be here!"); + break; + } + UDEBUG("Building FLANN index... done!"); + } + else + { + UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim()); + UASSERT(w->getDescriptor().type() == _flannIndex->featuresType()); + index = _flannIndex->addPoint(w->getDescriptor()); + } + std::pair::iterator, bool> inserted; + inserted = _mapIndexId.insert(std::pair(index, w->id())); + UASSERT(inserted.second); + inserted = _mapIdIndex.insert(std::pair(w->id(), index)); + UASSERT(inserted.second); + } + ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size()); } - - ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); } - UDEBUG("Dictionary updated! (size=%d->%d added=%d removed=%d)", - oldSize, _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); + else if(_strategy >= kNNBruteForce && + _notIndexedWords.size() && + _removedIndexedWords.size() == 0 && + _visualWords.size() && + _dataTree.rows) + { + //just add not indexed words + int i = _dataTree.rows; + _dataTree.reserve(_dataTree.rows + _notIndexedWords.size()); + for(std::set::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) + { + VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); + UASSERT(w); + UASSERT(w->getDescriptor().cols == _dataTree.cols); + UASSERT(w->getDescriptor().type() == _dataTree.type()); + _dataTree.push_back(w->getDescriptor()); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, w->id())); + std::pair::iterator, bool> inserted = _mapIdIndex.insert(std::pair(w->id(), i)); + UASSERT(inserted.second); + ++i; + } + } + else + { + _mapIndexId.clear(); + _mapIdIndex.clear(); + _dataTree = cv::Mat(); + _flannIndex->release(); + + if(_visualWords.size()) + { + UTimer timer; + timer.start(); + + int type = _visualWords.begin()->second->getDescriptor().type(); + int dim = _visualWords.begin()->second->getDescriptor().cols; + + UASSERT(type == CV_32F || type == CV_8U); + UASSERT(dim > 0); + + // Create the data matrix + _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F + std::map::const_iterator iter = _visualWords.begin(); + for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) + { + UASSERT(iter->second->getDescriptor().cols == dim); + UASSERT(iter->second->getDescriptor().type() == type); + + iter->second->getDescriptor().copyTo(_dataTree.row(i)); + _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); + _mapIdIndex.insert(_mapIdIndex.end(), std::pair(iter->second->id(), i)); + } + + ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim); + ULOGGER_DEBUG("copying data = %f s", timer.ticks()); + + switch(_strategy) + { + case kNNFlannNaive: + _flannIndex->build(_dataTree, rtflann::LinearIndexParams()); + break; + case kNNFlannKdTree: + UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); + _flannIndex->build(_dataTree, rtflann::KDTreeIndexParams()); + break; + case kNNFlannLSH: + UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); + _flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2)); + break; + default: + break; + } + + ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks()); + } + } + UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)", + _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size()); } else { @@ -346,6 +683,7 @@ void VWDictionary::update() } _notIndexedWords.clear(); _removedIndexedWords.clear(); + UDEBUG(""); } void VWDictionary::clear() @@ -370,6 +708,7 @@ void VWDictionary::clear() _lastWordId = 0; _dataTree = cv::Mat(); _mapIndexId.clear(); + _mapIdIndex.clear(); _unusedWords.clear(); _flannIndex->release(); } @@ -471,7 +810,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, UTimer timerLocal; timerLocal.start(); - if(!_dataTree.empty() && _dataTree.rows >= (int)k) + if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k)) { //Find nearest neighbors UDEBUG("newPts.total()=%d ", descriptors.rows); @@ -544,10 +883,15 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, { for(int j=0; j(i,j) >= 0) + float d = dists.at(i,j); + int id = uValue(_mapIndexId, results.at(i,j)); + if(d >= 0.0f && id > 0) { - float d = dists.at(i,j); - fullResults.insert(std::pair(d, uValue(_mapIndexId, results.at(i,j)))); + fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -555,10 +899,15 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, { for(unsigned int j=0; j= 0) + float d = matches.at(i).at(j).distance; + int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) { - float d = matches.at(i).at(j).distance; - fullResults.insert(std::pair(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx))); + fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -566,27 +915,21 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, // Check if this descriptor matches with a word from the last signature (a word not already added to the tree) if(_newWordsComparedTogether && newWords.rows) { - cv::flann::Index linearSeach; - linearSeach.build(newWords, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); - cv::Mat resultsLinear; - cv::Mat distsLinear; - linearSeach.knnSearch(descriptors.row(i), resultsLinear, distsLinear, newWords.rows>1?2:1); - // In case of binary descriptors - if(distsLinear.type() == CV_32S) + std::vector > matchesNewWords; + cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1); + UASSERT(matchesNewWords.size() == 1); + for(unsigned int j=0; j= 0.0f && id > 0) { - if(resultsLinear.at(0,j) >= 0) - { - float d = distsLinear.at(0,j); - fullResults.insert(std::pair(d, newWordsId[resultsLinear.at(0,j)])); - } + fullResults.insert(std::pair(d, id)); + } + else + { + break; } } } @@ -637,7 +980,6 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, this->addWordRef(fullResults.begin()->second, signatureId); wordIds.push_back(fullResults.begin()->second); - UASSERT(fullResults.begin()->second>0); } } else if(fullResults.size()) @@ -704,7 +1046,7 @@ std::vector VWDictionary::findNN(const std::list & vws) const } ULOGGER_DEBUG("Preparation time = %fs", timer.ticks()); - if(!_dataTree.empty() && _dataTree.rows >= (int)k) + if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k)) { //Find nearest neighbors UDEBUG("newPts.total()=%d ", query.total()); @@ -768,9 +1110,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const } ULOGGER_DEBUG("Search dictionary time = %fs", timer.ticks()); - cv::Mat resultsNotIndexed; - cv::Mat distsNotIndexed; std::map mapIndexIdNotIndexed; + std::vector > matchesNotIndexed; if(_notIndexedWords.size()) { cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type); @@ -786,16 +1127,8 @@ std::vector VWDictionary::findNN(const std::list & vws) const // Find nearest neighbor ULOGGER_DEBUG("Searching in words not indexed..."); - cv::flann::Index linearSeach; - linearSeach.build(dataNotIndexed, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING); - linearSeach.knnSearch(query, resultsNotIndexed, distsNotIndexed, _notIndexedWords.size()>1?2:1); - // In case of binary descriptors - if(distsNotIndexed.type() == CV_32S) - { - cv::Mat temp; - distsNotIndexed.convertTo(temp, CV_32F); - distsNotIndexed = temp; - } + cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1); } ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks()); @@ -806,10 +1139,11 @@ std::vector VWDictionary::findNN(const std::list & vws) const { for(int j=0; j(i,j) > 0) + float d = dists.at(i,j); + int id = uValue(_mapIndexId, results.at(i,j)); + if(d >= 0.0f && id > 0) { - float d = dists.at(i,j); - fullResults.insert(std::pair(d, uValue(_mapIndexId, results.at(i,j)))); + fullResults.insert(std::pair(d, id)); } } } @@ -817,21 +1151,30 @@ std::vector VWDictionary::findNN(const std::list & vws) const { for(unsigned int j=0; j 0) + float d = matches.at(i).at(j).distance; + int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) { - float d = matches.at(i).at(j).distance; - fullResults.insert(std::pair(d, uValue(_mapIndexId, matches.at(i).at(j).trainIdx))); + fullResults.insert(std::pair(d, id)); } } } // not indexed.. - for(int j=0; j(i,j) > 0) + for(unsigned int j=0; j(i,j); - fullResults.insert(std::pair(d, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at(i,j)))); + float d = matchesNotIndexed.at(i).at(j).distance; + int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx); + if(d >= 0.0f && id > 0) + { + fullResults.insert(std::pair(d, id)); + } + else + { + break; + } } } diff --git a/corelib/src/VisualWord.cpp b/corelib/src/VisualWord.cpp index b80cd6cc..9b54d4c4 100644 --- a/corelib/src/VisualWord.cpp +++ b/corelib/src/VisualWord.cpp @@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "VisualWord.h" +#include "rtabmap/core/VisualWord.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UStl.h" diff --git a/corelib/src/resources/DatabaseSchema.sql.in b/corelib/src/resources/DatabaseSchema.sql.in index 47051b2c..544d0d30 100644 --- a/corelib/src/resources/DatabaseSchema.sql.in +++ b/corelib/src/resources/DatabaseSchema.sql.in @@ -31,6 +31,7 @@ CREATE TABLE Data ( calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform scan BLOB, -- compressed data (Laser scan) scan_max_pts INTEGER, -- Laser scan max points + scan_max_range FLOAT, -- Laser max range user_data BLOB, -- compressed data (User data) time_enter DATE, PRIMARY KEY (id) @@ -39,10 +40,11 @@ CREATE TABLE Data ( CREATE TABLE Link ( from_id INTEGER NOT NULL, to_id INTEGER NOT NULL, - type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2 + type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2 rot_variance FLOAT NOT NULL, trans_variance FLOAT NOT NULL, transform BLOB, + user_data BLOB, -- compressed data (User data) FOREIGN KEY (from_id) REFERENCES Node(id), FOREIGN KEY (to_id) REFERENCES Node(id) ); @@ -81,7 +83,7 @@ CREATE TABLE Statistics ( ); CREATE TABLE Admin ( - version INTEGER, + version TEXT, time_enter DATE ); diff --git a/corelib/src/rtflann/algorithms/all_indices.h b/corelib/src/rtflann/algorithms/all_indices.h new file mode 100644 index 00000000..7f85665a --- /dev/null +++ b/corelib/src/rtflann/algorithms/all_indices.h @@ -0,0 +1,197 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 RTABMAP_FLANN_ALL_INDICES_H_ +#define RTABMAP_FLANN_ALL_INDICES_H_ + +#include "rtflann/general.h" + +#include "rtflann/algorithms/nn_index.h" +#include "rtflann/algorithms/kdtree_index.h" +#include "rtflann/algorithms/kdtree_single_index.h" +#include "rtflann/algorithms/kmeans_index.h" +#include "rtflann/algorithms/composite_index.h" +#include "rtflann/algorithms/linear_index.h" +#include "rtflann/algorithms/hierarchical_clustering_index.h" +#include "rtflann/algorithms/lsh_index.h" +#include "rtflann/algorithms/autotuned_index.h" +#ifdef FLANN_USE_CUDA +#include "rtflann/algorithms/kdtree_cuda_3d_index.h" +#endif + + +namespace rtflann +{ + +/** + * enable_if sfinae helper + */ +template struct enable_if{}; +template struct enable_if { typedef T type; }; + +/** + * disable_if sfinae helper + */ +template struct disable_if{ typedef T type; }; +template struct disable_if { }; + +/** + * Check if two type are the same + */ +template +struct same_type +{ + enum {value = false}; +}; + +template +struct same_type +{ + enum {value = true}; +}; + +#define HAS_MEMBER(member) \ + template \ + struct member { \ + typedef char No; \ + typedef long Yes; \ + template static Yes test( typename C::member* ); \ + template static No test( ... ); \ + enum { value = sizeof (test(0))==sizeof(Yes) }; \ + }; + +HAS_MEMBER(needs_kdtree_distance) +HAS_MEMBER(needs_vector_space_distance) +HAS_MEMBER(is_kdtree_distance) +HAS_MEMBER(is_vector_space_distance) + +struct DummyDistance +{ + typedef float ElementType; + typedef float ResultType; + + template + ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const + { + return ResultType(0); + } + + template + inline ResultType accum_dist(const U& a, const V& b, int) const + { + return ResultType(0); + } +}; + +/** + * Checks if an index and a distance can be used together + */ +template