├── .github ├── no-response.yml └── workflows │ └── build.yml ├── .gitignore ├── CMakeLists.txt ├── CMakeModules ├── FindBLAS.cmake ├── FindCSparse.cmake ├── FindCholmod.cmake ├── FindEigen.cmake ├── FindEigen3.cmake ├── FindG2O.cmake ├── FindLAPACK.cmake ├── FindQGLViewer.cmake └── FindSuiteSparse.cmake ├── LICENSE ├── README.MD ├── Thirdparty └── DBoW2 │ ├── CMakeLists.txt │ ├── DBoW2 │ ├── BowVector.cpp │ ├── BowVector.h │ ├── FClass.h │ ├── FORB.cpp │ ├── FORB.h │ ├── FeatureVector.cpp │ ├── FeatureVector.h │ ├── LICENSE.txt │ ├── ScoringObject.cpp │ ├── ScoringObject.h │ └── TemplatedVocabulary.h │ ├── DUtils │ ├── LICENSE.txt │ ├── Random.cpp │ ├── Random.h │ ├── Timestamp.cpp │ ├── Timestamp.h │ └── config.h │ └── LICENSE.txt ├── include └── se2clam │ ├── Config.h │ ├── Frame.h │ ├── FramePublish.h │ ├── GlobalMapper.h │ ├── KeyFrame.h │ ├── LocalMapper.h │ ├── Localizer.h │ ├── Map.h │ ├── MapPoint.h │ ├── MapPublish.h │ ├── MapStorage.h │ ├── ORBVocabulary.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── OdoSLAM.h │ ├── Sensors.h │ ├── Track.h │ ├── converter.h │ ├── optimizer.h │ ├── sparsifier.h │ └── sugarCV.h ├── package.xml ├── rviz.rviz ├── rviz.vcg ├── src ├── Config.cpp ├── Frame.cpp ├── FramePublish.cpp ├── GlobalMapper.cpp ├── KeyFrame.cpp ├── LocalMapper.cpp ├── Localizer.cpp ├── Map.cpp ├── MapPoint.cpp ├── MapPublish.cpp ├── MapStorage.cpp ├── ORBextractor.cpp ├── ORBmatcher.cpp ├── OdoSLAM.cpp ├── Sensors.cpp ├── Track.cpp ├── converter.cpp ├── optimizer.cpp ├── sparsifier.cpp └── sugarCV.cpp └── test ├── datapub.cpp ├── imgview.cpp ├── test_ros.cpp └── test_vn.cpp /.github/no-response.yml: -------------------------------------------------------------------------------- 1 | # Configuration for probot-no-response - https://github.com/probot/no-response 2 | 3 | # Number of days of inactivity before an Issue is closed for lack of response 4 | daysUntilClose: 14 5 | # Label requiring a response 6 | responseRequiredLabel: more-information-needed 7 | # Comment to post when closing an Issue for lack of response. Set to `false` to disable 8 | closeComment: > 9 | This issue has been automatically closed because there has been no response 10 | to our request for more information from the original author. 11 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-18.04 12 | 13 | steps: 14 | - uses: actions/checkout@v2 15 | - name: install required packages 16 | run: | 17 | sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 18 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 19 | sudo apt-get update 20 | sudo apt-get install -y libopencv-dev libeigen3-dev libsuitesparse-dev ros-melodic-desktop python-rosdep 21 | echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc 22 | source ~/.bashrc 23 | sudo rosdep init 24 | rosdep update 25 | 26 | - name: build 27 | run: | 28 | source /opt/ros/melodic/setup.bash 29 | mkdir build 30 | cd build 31 | git clone https://gitee.com/izhengfan/g2o_2016_installed.git 32 | sudo mv g2o_2016_installed/include/g2o /usr/local/include/g2o 33 | sudo mv g2o_2016_installed/lib/*.so /usr/local/lib/ 34 | cmake .. 35 | make 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | catkin_generated/* 2 | CMakeFiles/* 3 | cmake_install.cmake 4 | CTestTestfile.cmake 5 | Makefile 6 | odoslam.cbp 7 | CMakeLists.txt.user 8 | Makefile 9 | Thirdparty/DBoW2/build/ 10 | Thirdparty/DBoW2/lib/* 11 | *.bag 12 | **/*voc.bin 13 | log 14 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(se2clam) 3 | 4 | add_subdirectory(Thirdparty/DBoW2) 5 | 6 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules/) 7 | 8 | 9 | # c++ 11 support for g2o, multi-thread and shared_ptr 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wall") 11 | 12 | find_package(Eigen REQUIRED) 13 | find_package(OpenCV REQUIRED) 14 | find_package(CSparse REQUIRED) 15 | find_package(Cholmod REQUIRED) 16 | find_package(G2O REQUIRED) 17 | 18 | 19 | ## Find catkin macros and libraries 20 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 21 | ## is used, also find other catkin packages 22 | find_package(catkin REQUIRED COMPONENTS 23 | geometry_msgs 24 | nav_msgs 25 | roscpp 26 | rospy 27 | tf 28 | roslib 29 | cv_bridge 30 | cmake_modules 31 | image_transport 32 | message_generation 33 | ) 34 | #add_message_files( 35 | # FILES 36 | # SyncOdoImg.msg 37 | #) 38 | 39 | #generate_messages( 40 | # DEPENDENCIES 41 | # std_msgs 42 | # sensor_msgs 43 | # geometry_msgs 44 | #) 45 | 46 | catkin_package( 47 | # INCLUDE_DIRS include 48 | # LIBRARIES se2clam 49 | # CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy tf 50 | # DEPENDS Eigen OpenCV Boost 51 | 52 | # CATKIN_DEPENDS message_runtime 53 | ) 54 | 55 | 56 | include_directories( 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | list(APPEND LINK_LIBS 60 | ${catkin_LIBRARIES} 61 | ) 62 | # if do not use ros 63 | #find_package(Pangolin REQUIRED) 64 | #include_directories( 65 | # ${Pangolin_INCLUDE_DIRS} 66 | #) 67 | #list(APPEND LINK_LIBS 68 | # ${Pangolin_LIBRARIES} 69 | #) 70 | 71 | include_directories( 72 | include/se2clam/ 73 | ${CMAKE_CURRENT_SOURCE_DIR} 74 | ${EIGEN_INCLUDE_DIRS} 75 | ${OpenCV_INCLUDE_DIRS} 76 | ${CSPARSE_INCLUDE_DIR} 77 | ${Cholmod_INCLUDE_DIR} 78 | ${G2O_INCLUDE_DIR} 79 | ) 80 | 81 | list(APPEND LINK_LIBS 82 | ${OpenCV_LIBS} 83 | ) 84 | 85 | list(APPEND G2O_LIBS 86 | cxsparse 87 | cholmod 88 | g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d 89 | g2o_core g2o_solver_csparse g2o_solver_structure_only 90 | g2o_types_sba g2o_types_slam3d g2o_csparse_extension 91 | g2o_solver_dense g2o_stuff 92 | g2o_types_sclam2d g2o_solver_pcg 93 | g2o_types_data g2o_types_sim3 94 | ) 95 | 96 | ## Build a lib 97 | FILE(GLOB_RECURSE IncFiles "include/se2clam/*.h") 98 | aux_source_directory(src/. DIR_SRCS) 99 | add_library(se2clam ${DIR_SRCS} ${IncFiles}) 100 | 101 | ## Specify libraries to link a library or executable target against 102 | target_link_libraries(se2clam 103 | ${LINK_LIBS} 104 | ${G2O_LIBS} 105 | DBoW2se2clam 106 | ) 107 | 108 | ## Build an exe 109 | add_executable(test_vn test/test_vn.cpp) 110 | 111 | ## Specify libraries to link a library or executable target against 112 | target_link_libraries(test_vn 113 | se2clam 114 | ${LINK_LIBS} 115 | ${G2O_LIBS} 116 | #${CMAKE_CURRENT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so 117 | ) 118 | 119 | 120 | add_executable(datapub test/datapub.cpp) 121 | target_link_libraries(datapub 122 | se2clam 123 | ${LINK_LIBS} 124 | ) 125 | 126 | add_executable(imgview test/imgview.cpp) 127 | target_link_libraries(imgview 128 | se2clam 129 | ${LINK_LIBS} 130 | ) 131 | 132 | #add_executable(simpletest test/simpletest.cpp) 133 | #target_link_libraries(simpletest 134 | #se2clam 135 | #${LINK_LIBS} 136 | #) 137 | 138 | ## Build a publisher for vn dataset 139 | #add_executable(publish_vn test/publish_vn.cpp) 140 | #target_link_libraries(publish_vn 141 | #${LINK_LIBS} 142 | #se2clam 143 | #) 144 | 145 | 146 | ## Build a subscriber for vn dataset 147 | #add_executable(subscribe_vn test/subscribe_vn.cpp) 148 | #target_link_libraries(subscribe_vn 149 | #${LINK_LIBS} 150 | #se2clam 151 | #) 152 | 153 | # Build a subscriber test for vn dataset 154 | add_executable(test_ros test/test_ros.cpp) 155 | target_link_libraries(test_ros 156 | se2clam 157 | ${LINK_LIBS} 158 | ) 159 | -------------------------------------------------------------------------------- /CMakeModules/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | ) 14 | 15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 16 | PATHS 17 | /usr/lib 18 | /usr/local/lib 19 | /opt/local/lib 20 | /sw/lib 21 | ) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 25 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 26 | -------------------------------------------------------------------------------- /CMakeModules/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # 3 | # CMake script for finding the Eigen library. 4 | # 5 | # http://eigen.tuxfamily.org/index.php?title=Main_Page 6 | # 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /CMakeModules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /CMakeModules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /CMakeModules/FindQGLViewer.cmake: -------------------------------------------------------------------------------- 1 | # Need to find both Qt{4,5} and QGLViewer if the QQL support is to be built 2 | FIND_PACKAGE(Qt4 COMPONENTS QtCore QtXml QtOpenGL QtGui) 3 | IF(NOT Qt4_FOUND) 4 | FIND_PACKAGE(Qt5 QUIET COMPONENTS Core Xml OpenGL Gui Widgets) 5 | IF(NOT Qt4_FOUND AND NOT Qt5_FOUND) 6 | MESSAGE("Qt{4,5} not found. Install it and set Qt{4,5}_DIR accordingly") 7 | IF (WIN32) 8 | MESSAGE(" In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5") 9 | ENDIF() 10 | ENDIF() 11 | ENDIF() 12 | 13 | FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h 14 | /usr/include/QGLViewer 15 | /opt/local/include/QGLViewer 16 | /usr/local/include/QGLViewer 17 | /sw/include/QGLViewer 18 | ENV QGLVIEWERROOT 19 | ) 20 | 21 | find_library(QGLVIEWER_LIBRARY_RELEASE 22 | NAMES qglviewer-qt4 qglviewer QGLViewer QGLViewer2 23 | PATHS /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ENV QGLVIEWERROOT 28 | ENV LD_LIBRARY_PATH 29 | ENV LIBRARY_PATH 30 | PATH_SUFFIXES QGLViewer QGLViewer/release 31 | ) 32 | find_library(QGLVIEWER_LIBRARY_DEBUG 33 | NAMES dqglviewer dQGLViewer dQGLViewer2 QGLViewerd2 34 | PATHS /usr/lib 35 | /usr/local/lib 36 | /opt/local/lib 37 | /sw/lib 38 | ENV QGLVIEWERROOT 39 | ENV LD_LIBRARY_PATH 40 | ENV LIBRARY_PATH 41 | PATH_SUFFIXES QGLViewer QGLViewer/release 42 | ) 43 | 44 | if(QGLVIEWER_LIBRARY_RELEASE) 45 | if(QGLVIEWER_LIBRARY_DEBUG) 46 | set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG}) 47 | else() 48 | set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE}) 49 | endif() 50 | endif() 51 | 52 | include(FindPackageHandleStandardArgs) 53 | find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG 54 | QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY) 55 | -------------------------------------------------------------------------------- /CMakeModules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 ZHENG Fan 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.MD: -------------------------------------------------------------------------------- 1 | se2clam 2 | --- 3 | SE(2)-Constrained Localization and Mapping by Fusing Odometry and Vision 4 | 5 | [![](../../workflows/Build/badge.svg)](../../actions?query=workflow%3A"Build") 6 | 7 | ### Related Publication 8 | 9 | - Fan Zheng, Hengbo Tang, Yun-Hui Liu. "Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses". _IEEE Transactions on Cybernetics_, vol. 49, no. 7, 2019 10 | 11 | To cite it in bib: 12 | ``` 13 | @article{fzheng2018tcyb, 14 | author = {Fan Zheng and Hengbo Tang and Yun-Hui Liu}, 15 | journal = {IEEE Trans. Cybernetics}, 16 | title = "{Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses}", 17 | volume = {49}, 18 | number = {7}, 19 | year = {2019}, 20 | } 21 | ``` 22 | 23 | 24 | ### Dependencies 25 | 26 | - ROS (tested on Kinetic/Melodic) 27 | 28 | - OpenCV 2.4.x / 3.1 above 29 | 30 | - g2o ([2016 version](https://github.com/RainerKuemmerle/g2o/releases/tag/20160424_git)) 31 | 32 | ### Build 33 | 34 | Build this project as a ROS package 35 | 36 | ### Demo 37 | 38 | 1. Download [DatasetRoom.zip](https://mycuhk-my.sharepoint.com/:u:/g/personal/1155051778_link_cuhk_edu_hk/Ef4NuXvLZI1JhfljH9LkNxUB5xrDrCOrRnxwztO5bGKlew?e=U4aind), and extract it. In a terminal, `cd` into `DatasetRoom/`. 39 | 40 | We prepare two packages of odometry measurement data, one is more accurate (`odo_raw_accu.txt`), the other less accurate (`odo_raw_roug.txt`). To use either one of them, copy it to `odo_raw.txt` in `DatasetRoom/`. 41 | 42 | 2. Download [ORBvoc.bin](https://mycuhk-my.sharepoint.com/:u:/g/personal/1155051778_link_cuhk_edu_hk/EaF2ZkP17rdJrUHT0mrcf74Bl1h_691xZrxNILGbQbYFmA?e=nXRSS4). 43 | 44 | 3. Run rviz: 45 | 46 | ``` 47 | roscd se2clam 48 | rosrun rviz rviz -d rviz.rviz 49 | ``` 50 | 51 | 4. Run se2clam: 52 | 53 | ``` 54 | rosrun se2clam test_vn PATH_TO_DatasetRoom PATH_TO_ORBvoc.bin 55 | ``` 56 | 57 | ![result in rviz](https://images.gitee.com/uploads/images/2019/0304/231649_cdb48a5e_874043.png) 58 | 59 | 60 | ### Related Project 61 | 62 | [izhengfan/se2lam](https://github.com/izhengfan/se2lam) 63 | [izhengfan/ORB_SLAM2](https://github.com/izhengfan/ORB_SLAM2) 64 | 65 | ### License 66 | 67 | [MIT](LICENSE) 68 | 69 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2se2clam) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | find_package(OpenCV REQUIRED) 28 | 29 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 30 | 31 | include_directories(${OpenCV_INCLUDE_DIRS}) 32 | add_library(DBoW2se2clam SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 33 | target_link_libraries(DBoW2se2clam ${OpenCV_LIBS}) 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Id of words 22 | typedef unsigned int WordId; 23 | 24 | /// Value of a word 25 | typedef double WordValue; 26 | 27 | /// Id of nodes in the vocabulary treee 28 | typedef unsigned int NodeId; 29 | 30 | /// L-norms for normalization 31 | EXPORT typedef enum LNorm 32 | { 33 | L1, 34 | L2 35 | } LNorm; 36 | 37 | /// Weighting type 38 | EXPORT typedef enum WeightingType 39 | { 40 | TF_IDF, 41 | TF, 42 | IDF, 43 | BINARY 44 | } WeightingType; 45 | 46 | /// Scoring type 47 | EXPORT typedef enum ScoringType 48 | { 49 | L1_NORM, 50 | L2_NORM, 51 | CHI_SQUARE, 52 | KL, 53 | BHATTACHARYYA, 54 | DOT_PRODUCT, 55 | } ScoringType; 56 | 57 | /// Vector of words to represent images 58 | /// stl的map结构，key为wordId，value为tfidf中的tf 59 | class EXPORT BowVector: 60 | public std::map 61 | { 62 | public: 63 | 64 | /** 65 | * Constructor 66 | */ 67 | BowVector(void); 68 | 69 | /** 70 | * Destructor 71 | */ 72 | ~BowVector(void); 73 | 74 | /** 75 | * Adds a value to a word value existing in the vector, or creates a new 76 | * word with the given value 77 | * @param id word id to look for 78 | * @param v value to create the word with, or to add to existing word 79 | */ 80 | void addWeight(WordId id, WordValue v); 81 | 82 | /** 83 | * Adds a word with a value to the vector only if this does not exist yet 84 | * @param id word id to look for 85 | * @param v value to give to the word if this does not exist 86 | */ 87 | void addIfNotExist(WordId id, WordValue v); 88 | 89 | /** 90 | * L1-Normalizes the values in the vector 91 | * @param norm_type norm used 92 | */ 93 | void normalize(LNorm norm_type); 94 | 95 | /** 96 | * Prints the content of the bow vector 97 | * @param out stream 98 | * @param v 99 | */ 100 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 101 | 102 | /** 103 | * Saves the bow vector as a vector in a matlab file 104 | * @param filename 105 | * @param W number of words in the vocabulary 106 | */ 107 | void saveM(const std::string &filename, size_t W) const; 108 | }; 109 | 110 | } // namespace DBoW2 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "../DUtils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Generic class to encapsulate functions to manage descriptors. 22 | /** 23 | * This class must be inherited. Derived classes can be used as the 24 | * parameter F when creating Templated structures 25 | * (TemplatedVocabulary, TemplatedDatabase, ...) 26 | */ 27 | class EXPORT FClass 28 | { 29 | class TDescriptor; 30 | typedef const TDescriptor *pDescriptor; 31 | 32 | /** 33 | * Calculates the mean value of a set of descriptors 34 | * @param descriptors 35 | * @param mean mean descriptor 36 | */ 37 | virtual void meanValue(const std::vector &descriptors, 38 | TDescriptor &mean) = 0; 39 | 40 | /** 41 | * Calculates the distance between two descriptors 42 | * @param a 43 | * @param b 44 | * @return distance 45 | */ 46 | static double distance(const TDescriptor &a, const TDescriptor &b); 47 | 48 | /** 49 | * Returns a string version of the descriptor 50 | * @param a descriptor 51 | * @return string version 52 | */ 53 | static std::string toString(const TDescriptor &a); 54 | 55 | /** 56 | * Returns a descriptor from a string 57 | * @param a descriptor 58 | * @param s string version 59 | */ 60 | static void fromString(TDescriptor &a, const std::string &s); 61 | 62 | /** 63 | * Returns a mat with the descriptors in float format 64 | * @param descriptors 65 | * @param mat (out) NxL 32F matrix 66 | */ 67 | static void toMat32F(const std::vector &descriptors, 68 | cv::Mat &mat); 69 | }; 70 | 71 | } // namespace DBoW2 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.cpp 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | * Distance function has been modified 9 | * 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "FORB.h" 20 | 21 | using namespace std; 22 | 23 | namespace DBoW2 { 24 | 25 | // -------------------------------------------------------------------------- 26 | 27 | // const int FORB::L=32; 28 | 29 | void FORB::meanValue(const std::vector &descriptors, 30 | FORB::TDescriptor &mean) 31 | { 32 | if(descriptors.empty()) 33 | { 34 | mean.release(); 35 | return; 36 | } 37 | else if(descriptors.size() == 1) 38 | { 39 | mean = descriptors[0]->clone(); 40 | } 41 | else 42 | { 43 | vector sum(FORB::L * 8, 0); 44 | 45 | for(size_t i = 0; i < descriptors.size(); ++i) 46 | { 47 | const cv::Mat &d = *descriptors[i]; 48 | const unsigned char *p = d.ptr(); 49 | 50 | for(int j = 0; j < d.cols; ++j, ++p) 51 | { 52 | if(*p & (1 << 7)) ++sum[ j*8 ]; 53 | if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; 54 | if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; 55 | if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; 56 | if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; 57 | if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; 58 | if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; 59 | if(*p & (1)) ++sum[ j*8 + 7 ]; 60 | } 61 | } 62 | 63 | mean = cv::Mat::zeros(1, FORB::L, CV_8U); 64 | unsigned char *p = mean.ptr(); 65 | 66 | const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; 67 | for(size_t i = 0; i < sum.size(); ++i) 68 | { 69 | if(sum[i] >= N2) 70 | { 71 | // set bit 72 | *p |= 1 << (7 - (i % 8)); 73 | } 74 | 75 | if(i % 8 == 7) ++p; 76 | } 77 | } 78 | } 79 | 80 | // -------------------------------------------------------------------------- 81 | 82 | int FORB::distance(const FORB::TDescriptor &a, 83 | const FORB::TDescriptor &b) 84 | { 85 | // Bit set count operation from 86 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 87 | 88 | const int *pa = a.ptr(); 89 | const int *pb = b.ptr(); 90 | 91 | int dist=0; 92 | 93 | for(int i=0; i<8; i++, pa++, pb++) 94 | { 95 | unsigned int v = *pa ^ *pb; 96 | v = v - ((v >> 1) & 0x55555555); 97 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 98 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 99 | } 100 | 101 | return dist; 102 | } 103 | 104 | // -------------------------------------------------------------------------- 105 | 106 | std::string FORB::toString(const FORB::TDescriptor &a) 107 | { 108 | stringstream ss; 109 | const unsigned char *p = a.ptr(); 110 | 111 | for(int i = 0; i < a.cols; ++i, ++p) 112 | { 113 | ss << (int)*p << " "; 114 | } 115 | 116 | return ss.str(); 117 | } 118 | 119 | // -------------------------------------------------------------------------- 120 | 121 | void FORB::fromString(FORB::TDescriptor &a, const std::string &s) 122 | { 123 | a.create(1, FORB::L, CV_8U); 124 | unsigned char *p = a.ptr(); 125 | 126 | stringstream ss(s); 127 | for(int i = 0; i < FORB::L; ++i, ++p) 128 | { 129 | int n; 130 | ss >> n; 131 | 132 | if(!ss.fail()) 133 | *p = (unsigned char)n; 134 | } 135 | 136 | } 137 | 138 | // -------------------------------------------------------------------------- 139 | 140 | void FORB::toMat32F(const std::vector &descriptors, 141 | cv::Mat &mat) 142 | { 143 | if(descriptors.empty()) 144 | { 145 | mat.release(); 146 | return; 147 | } 148 | 149 | const size_t N = descriptors.size(); 150 | 151 | mat.create(N, FORB::L*8, CV_32F); 152 | float *p = mat.ptr(); 153 | 154 | for(size_t i = 0; i < N; ++i) 155 | { 156 | const int C = descriptors[i].cols; 157 | const unsigned char *desc = descriptors[i].ptr(); 158 | 159 | for(int j = 0; j < C; ++j, p += 8) 160 | { 161 | p[0] = (desc[j] & (1 << 7) ? 1 : 0); 162 | p[1] = (desc[j] & (1 << 6) ? 1 : 0); 163 | p[2] = (desc[j] & (1 << 5) ? 1 : 0); 164 | p[3] = (desc[j] & (1 << 4) ? 1 : 0); 165 | p[4] = (desc[j] & (1 << 3) ? 1 : 0); 166 | p[5] = (desc[j] & (1 << 2) ? 1 : 0); 167 | p[6] = (desc[j] & (1 << 1) ? 1 : 0); 168 | p[7] = desc[j] & (1); 169 | } 170 | } 171 | } 172 | 173 | // -------------------------------------------------------------------------- 174 | 175 | void FORB::toMat8U(const std::vector &descriptors, 176 | cv::Mat &mat) 177 | { 178 | mat.create(descriptors.size(), 32, CV_8U); 179 | 180 | unsigned char *p = mat.ptr(); 181 | 182 | for(size_t i = 0; i < descriptors.size(); ++i, p += 32) 183 | { 184 | const unsigned char *d = descriptors[i].ptr(); 185 | std::copy(d, d+32, p); 186 | } 187 | 188 | } 189 | 190 | // -------------------------------------------------------------------------- 191 | 192 | } // namespace DBoW2 193 | 194 | 195 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | #include "../DUtils/config.h" 20 | 21 | namespace DBoW2 { 22 | 23 | /// Functions to manipulate ORB descriptors 24 | class EXPORT FORB : protected FClass 25 | { 26 | public: 27 | 28 | /// Descriptor type 29 | typedef cv::Mat TDescriptor; // CV_8U 30 | /// Pointer to a single descriptor 31 | typedef const TDescriptor *pDescriptor; 32 | /// Descriptor length (in bytes) 33 | static const int L = 32; 34 | 35 | /** 36 | * Calculates the mean value of a set of descriptors 37 | * @param descriptors 38 | * @param mean mean descriptor 39 | */ 40 | static void meanValue(const std::vector &descriptors, 41 | TDescriptor &mean); 42 | 43 | /** 44 | * Calculates the distance between two descriptors 45 | * @param a 46 | * @param b 47 | * @return distance 48 | */ 49 | static int distance(const TDescriptor &a, const TDescriptor &b); 50 | 51 | /** 52 | * Returns a string version of the descriptor 53 | * @param a descriptor 54 | * @return string version 55 | */ 56 | static std::string toString(const TDescriptor &a); 57 | 58 | /** 59 | * Returns a descriptor from a string 60 | * @param a descriptor 61 | * @param s string version 62 | */ 63 | static void fromString(TDescriptor &a, const std::string &s); 64 | 65 | /** 66 | * Returns a mat with the descriptors in float format 67 | * @param descriptors 68 | * @param mat (out) NxL 32F matrix 69 | */ 70 | static void toMat32F(const std::vector &descriptors, 71 | cv::Mat &mat); 72 | 73 | static void toMat8U(const std::vector &descriptors, 74 | cv::Mat &mat); 75 | 76 | }; 77 | 78 | } // namespace DBoW2 79 | 80 | #endif 81 | 82 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include "../DUtils/config.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Vector of nodes with indexes of local features 23 | class EXPORT FeatureVector: 24 | public std::map > 25 | { 26 | public: 27 | 28 | /** 29 | * Constructor 30 | */ 31 | FeatureVector(void); 32 | 33 | /** 34 | * Destructor 35 | */ 36 | ~FeatureVector(void); 37 | 38 | /** 39 | * Adds a feature to an existing node, or adds a new node with an initial 40 | * feature 41 | * @param id node id to add or to modify 42 | * @param i_feature index of feature to add to the given node 43 | */ 44 | void addFeature(NodeId id, unsigned int i_feature); 45 | 46 | /** 47 | * Sends a string versions of the feature vector through the stream 48 | * @param out stream 49 | * @param v feature vector 50 | */ 51 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 52 | 53 | }; 54 | 55 | } // namespace DBoW2 56 | 57 | #endif 58 | 59 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of: 2 | 3 | DBoW2: bag-of-words library for C++ with generic descriptors 4 | 5 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 6 | All rights reserved. 7 | 8 | This version contains only a subset of the original components of the library 9 | and FORB.cpp has been modified (see the file for details). 10 | 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions 14 | are met: 15 | 1. Redistributions of source code must retain the above copyright 16 | notice, this list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 20 | 3. The original author of the work must be notified of any 21 | redistribution of source code or in binary form. 22 | 4. Neither the name of copyright holders nor the names of its 23 | contributors may be used to endorse or promote products derived 24 | from this software without specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 30 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 32 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 33 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 34 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 35 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | POSSIBILITY OF SUCH DAMAGE. 37 | 38 | If you use it in an academic work, please cite: 39 | 40 | @ARTICLE{GalvezTRO12, 41 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 42 | journal={IEEE Transactions on Robotics}, 43 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 44 | year={2012}, 45 | month={October}, 46 | volume={28}, 47 | number={5}, 48 | pages={1188--1197}, 49 | doi={10.1109/TRO.2012.2197158}, 50 | ISSN={1552-3098} 51 | } 52 | 53 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include "TemplatedVocabulary.h" 12 | #include "BowVector.h" 13 | 14 | using namespace DBoW2; 15 | 16 | // If you change the type of WordValue, make sure you change also the 17 | // epsilon value (this is needed by the KL method) 18 | const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON 19 | 20 | // --------------------------------------------------------------------------- 21 | // --------------------------------------------------------------------------- 22 | 23 | double L1Scoring::score(const BowVector &v1, const BowVector &v2) const 24 | { 25 | BowVector::const_iterator v1_it, v2_it; 26 | const BowVector::const_iterator v1_end = v1.end(); 27 | const BowVector::const_iterator v2_end = v2.end(); 28 | 29 | v1_it = v1.begin(); 30 | v2_it = v2.begin(); 31 | 32 | double score = 0; 33 | 34 | while(v1_it != v1_end && v2_it != v2_end) 35 | { 36 | const WordValue& vi = v1_it->second; 37 | const WordValue& wi = v2_it->second; 38 | 39 | if(v1_it->first == v2_it->first) 40 | { 41 | score += fabs(vi - wi) - fabs(vi) - fabs(wi); 42 | 43 | // move v1 and v2 forward 44 | ++v1_it; 45 | ++v2_it; 46 | } 47 | else if(v1_it->first < v2_it->first) 48 | { 49 | // move v1 forward 50 | v1_it = v1.lower_bound(v2_it->first); 51 | // v1_it = (first element >= v2_it.id) 52 | } 53 | else 54 | { 55 | // move v2 forward 56 | v2_it = v2.lower_bound(v1_it->first); 57 | // v2_it = (first element >= v1_it.id) 58 | } 59 | } 60 | 61 | // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) 62 | // for all i | v_i != 0 and w_i != 0 63 | // (Nister, 2006) 64 | // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} 65 | score = -score/2.0; 66 | 67 | return score; // [0..1] 68 | } 69 | 70 | // --------------------------------------------------------------------------- 71 | // --------------------------------------------------------------------------- 72 | 73 | double L2Scoring::score(const BowVector &v1, const BowVector &v2) const 74 | { 75 | BowVector::const_iterator v1_it, v2_it; 76 | const BowVector::const_iterator v1_end = v1.end(); 77 | const BowVector::const_iterator v2_end = v2.end(); 78 | 79 | v1_it = v1.begin(); 80 | v2_it = v2.begin(); 81 | 82 | double score = 0; 83 | 84 | while(v1_it != v1_end && v2_it != v2_end) 85 | { 86 | const WordValue& vi = v1_it->second; 87 | const WordValue& wi = v2_it->second; 88 | 89 | if(v1_it->first == v2_it->first) 90 | { 91 | score += vi * wi; 92 | 93 | // move v1 and v2 forward 94 | ++v1_it; 95 | ++v2_it; 96 | } 97 | else if(v1_it->first < v2_it->first) 98 | { 99 | // move v1 forward 100 | v1_it = v1.lower_bound(v2_it->first); 101 | // v1_it = (first element >= v2_it.id) 102 | } 103 | else 104 | { 105 | // move v2 forward 106 | v2_it = v2.lower_bound(v1_it->first); 107 | // v2_it = (first element >= v1_it.id) 108 | } 109 | } 110 | 111 | // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) 112 | // for all i | v_i != 0 and w_i != 0 ) 113 | // (Nister, 2006) 114 | if(score >= 1) // rounding errors 115 | score = 1.0; 116 | else 117 | score = 1.0 - sqrt(1.0 - score); // [0..1] 118 | 119 | return score; 120 | } 121 | 122 | // --------------------------------------------------------------------------- 123 | // --------------------------------------------------------------------------- 124 | 125 | double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) 126 | const 127 | { 128 | BowVector::const_iterator v1_it, v2_it; 129 | const BowVector::const_iterator v1_end = v1.end(); 130 | const BowVector::const_iterator v2_end = v2.end(); 131 | 132 | v1_it = v1.begin(); 133 | v2_it = v2.begin(); 134 | 135 | double score = 0; 136 | 137 | // all the items are taken into account 138 | 139 | while(v1_it != v1_end && v2_it != v2_end) 140 | { 141 | const WordValue& vi = v1_it->second; 142 | const WordValue& wi = v2_it->second; 143 | 144 | if(v1_it->first == v2_it->first) 145 | { 146 | // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) 147 | // we move the -4 out 148 | if(vi + wi != 0.0) score += vi * wi / (vi + wi); 149 | 150 | // move v1 and v2 forward 151 | ++v1_it; 152 | ++v2_it; 153 | } 154 | else if(v1_it->first < v2_it->first) 155 | { 156 | // move v1 forward 157 | v1_it = v1.lower_bound(v2_it->first); 158 | } 159 | else 160 | { 161 | // move v2 forward 162 | v2_it = v2.lower_bound(v1_it->first); 163 | } 164 | } 165 | 166 | // this takes the -4 into account 167 | score = 2. * score; // [0..1] 168 | 169 | return score; 170 | } 171 | 172 | // --------------------------------------------------------------------------- 173 | // --------------------------------------------------------------------------- 174 | 175 | double KLScoring::score(const BowVector &v1, const BowVector &v2) const 176 | { 177 | BowVector::const_iterator v1_it, v2_it; 178 | const BowVector::const_iterator v1_end = v1.end(); 179 | const BowVector::const_iterator v2_end = v2.end(); 180 | 181 | v1_it = v1.begin(); 182 | v2_it = v2.begin(); 183 | 184 | double score = 0; 185 | 186 | // all the items or v are taken into account 187 | 188 | while(v1_it != v1_end && v2_it != v2_end) 189 | { 190 | const WordValue& vi = v1_it->second; 191 | const WordValue& wi = v2_it->second; 192 | 193 | if(v1_it->first == v2_it->first) 194 | { 195 | if(vi != 0 && wi != 0) score += vi * log(vi/wi); 196 | 197 | // move v1 and v2 forward 198 | ++v1_it; 199 | ++v2_it; 200 | } 201 | else if(v1_it->first < v2_it->first) 202 | { 203 | // move v1 forward 204 | score += vi * (log(vi) - LOG_EPS); 205 | ++v1_it; 206 | } 207 | else 208 | { 209 | // move v2_it forward, do not add any score 210 | v2_it = v2.lower_bound(v1_it->first); 211 | // v2_it = (first element >= v1_it.id) 212 | } 213 | } 214 | 215 | // sum rest of items of v 216 | for(; v1_it != v1_end; ++v1_it) 217 | if(v1_it->second != 0) 218 | score += v1_it->second * (log(v1_it->second) - LOG_EPS); 219 | 220 | return score; // cannot be scaled 221 | } 222 | 223 | // --------------------------------------------------------------------------- 224 | // --------------------------------------------------------------------------- 225 | 226 | double BhattacharyyaScoring::score(const BowVector &v1, 227 | const BowVector &v2) const 228 | { 229 | BowVector::const_iterator v1_it, v2_it; 230 | const BowVector::const_iterator v1_end = v1.end(); 231 | const BowVector::const_iterator v2_end = v2.end(); 232 | 233 | v1_it = v1.begin(); 234 | v2_it = v2.begin(); 235 | 236 | double score = 0; 237 | 238 | while(v1_it != v1_end && v2_it != v2_end) 239 | { 240 | const WordValue& vi = v1_it->second; 241 | const WordValue& wi = v2_it->second; 242 | 243 | if(v1_it->first == v2_it->first) 244 | { 245 | score += sqrt(vi * wi); 246 | 247 | // move v1 and v2 forward 248 | ++v1_it; 249 | ++v2_it; 250 | } 251 | else if(v1_it->first < v2_it->first) 252 | { 253 | // move v1 forward 254 | v1_it = v1.lower_bound(v2_it->first); 255 | // v1_it = (first element >= v2_it.id) 256 | } 257 | else 258 | { 259 | // move v2 forward 260 | v2_it = v2.lower_bound(v1_it->first); 261 | // v2_it = (first element >= v1_it.id) 262 | } 263 | } 264 | 265 | return score; // already scaled 266 | } 267 | 268 | // --------------------------------------------------------------------------- 269 | // --------------------------------------------------------------------------- 270 | 271 | double DotProductScoring::score(const BowVector &v1, 272 | const BowVector &v2) const 273 | { 274 | BowVector::const_iterator v1_it, v2_it; 275 | const BowVector::const_iterator v1_end = v1.end(); 276 | const BowVector::const_iterator v2_end = v2.end(); 277 | 278 | v1_it = v1.begin(); 279 | v2_it = v2.begin(); 280 | 281 | double score = 0; 282 | 283 | while(v1_it != v1_end && v2_it != v2_end) 284 | { 285 | const WordValue& vi = v1_it->second; 286 | const WordValue& wi = v2_it->second; 287 | 288 | if(v1_it->first == v2_it->first) 289 | { 290 | score += vi * wi; 291 | 292 | // move v1 and v2 forward 293 | ++v1_it; 294 | ++v2_it; 295 | } 296 | else if(v1_it->first < v2_it->first) 297 | { 298 | // move v1 forward 299 | v1_it = v1.lower_bound(v2_it->first); 300 | // v1_it = (first element >= v2_it.id) 301 | } 302 | else 303 | { 304 | // move v2 forward 305 | v2_it = v2.lower_bound(v1_it->first); 306 | // v2_it = (first element >= v1_it.id) 307 | } 308 | } 309 | 310 | return score; // cannot scale 311 | } 312 | 313 | // --------------------------------------------------------------------------- 314 | // --------------------------------------------------------------------------- 315 | 316 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | #include "../DUtils/config.h" 16 | 17 | namespace DBoW2 { 18 | 19 | /// Base class of scoring functions 20 | class EXPORT GeneralScoring 21 | { 22 | public: 23 | /** 24 | * Computes the score between two vectors. Vectors must be sorted and 25 | * normalized if necessary 26 | * @param v (in/out) 27 | * @param w (in/out) 28 | * @return score 29 | */ 30 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 31 | 32 | /** 33 | * Returns whether a vector must be normalized before scoring according 34 | * to the scoring scheme 35 | * @param norm norm to use 36 | * @return true iff must normalize 37 | */ 38 | virtual bool mustNormalize(LNorm &norm) const = 0; 39 | 40 | /// Log of epsilon 41 | static const double LOG_EPS; 42 | // If you change the type of WordValue, make sure you change also the 43 | // epsilon value (this is needed by the KL method) 44 | 45 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 46 | 47 | }; 48 | 49 | /** 50 | * Macro for defining Scoring classes 51 | * @param NAME name of class 52 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 53 | * @param NORM type of norm to use when MUSTNORMALIZE 54 | */ 55 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 56 | NAME: public GeneralScoring \ 57 | { public: \ 58 | /** \ 59 | * Computes score between two vectors \ 60 | * @param v \ 61 | * @param w \ 62 | * @return score between v and w \ 63 | */ \ 64 | virtual double score(const BowVector &v, const BowVector &w) const; \ 65 | \ 66 | /** \ 67 | * Says if a vector must be normalized according to the scoring function \ 68 | * @param norm (out) if true, norm to use 69 | * @return true iff vectors must be normalized \ 70 | */ \ 71 | virtual inline bool mustNormalize(LNorm &norm) const \ 72 | { norm = NORM; return MUSTNORMALIZE; } \ 73 | } 74 | 75 | /// L1 Scoring object 76 | class EXPORT __SCORING_CLASS(L1Scoring, true, L1); 77 | 78 | /// L2 Scoring object 79 | class EXPORT __SCORING_CLASS(L2Scoring, true, L2); 80 | 81 | /// Chi square Scoring object 82 | class EXPORT __SCORING_CLASS(ChiSquareScoring, true, L1); 83 | 84 | /// KL divergence Scoring object 85 | class EXPORT __SCORING_CLASS(KLScoring, true, L1); 86 | 87 | /// Bhattacharyya Scoring object 88 | class EXPORT __SCORING_CLASS(BhattacharyyaScoring, true, L1); 89 | 90 | /// Dot product Scoring object 91 | class EXPORT __SCORING_CLASS(DotProductScoring, false, L1); 92 | 93 | #undef __SCORING_CLASS 94 | 95 | } // namespace DBoW2 96 | 97 | #endif 98 | 99 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of DLib: 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 4 | All rights reserved. 5 | 6 | This version only contains a subset of the original components of 7 | the library. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions 11 | are met: 12 | 1. Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 2. Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | 3. The original author of the work must be notified of any 18 | redistribution of source code or in binary form. 19 | 4. Neither the name of copyright holders nor the names of its 20 | contributors may be used to endorse or promote products derived 21 | from this software without specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 25 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 26 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 27 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | POSSIBILITY OF SUCH DAMAGE. 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/config.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONFIG_H 2 | #define __CONFIG_H 3 | 4 | #ifdef WIN32 5 | #ifndef DBoW2_EXPORTS 6 | #define DBoW2_EXPORTS 7 | #endif 8 | #ifdef DBoW2_EXPORTS 9 | #define EXPORT //__declspec(dllexport) 10 | #else 11 | #define EXPORT //__declspec(dllimport) 12 | #endif 13 | #else 14 | #define EXPORT 15 | #endif 16 | 17 | #endif // CONFIG_H 18 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of: 2 | 3 | DBoW2: bag-of-words library for C++ with generic descriptors 4 | 5 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 6 | All rights reserved. 7 | 8 | This version contains only a subset of the original components of the library 9 | and DBoW2/FORB.cpp has been modified (see the file for details). 10 | 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions 14 | are met: 15 | 1. Redistributions of source code must retain the above copyright 16 | notice, this list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 20 | 3. The original author of the work must be notified of any 21 | redistribution of source code or in binary form. 22 | 4. Neither the name of copyright holders nor the names of its 23 | contributors may be used to endorse or promote products derived 24 | from this software without specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 30 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 32 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 33 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 34 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 35 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | POSSIBILITY OF SUCH DAMAGE. 37 | 38 | If you use it in an academic work, please cite: 39 | 40 | @ARTICLE{GalvezTRO12, 41 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 42 | journal={IEEE Transactions on Robotics}, 43 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 44 | year={2012}, 45 | month={October}, 46 | volume={28}, 47 | number={5}, 48 | pages={1188--1197}, 49 | doi={10.1109/TRO.2012.2197158}, 50 | ISSN={1552-3098} 51 | } 52 | 53 | -------------------------------------------------------------------------------- /include/se2clam/Config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef CONFIG_H 6 | #define CONFIG_H 7 | 8 | #include 9 | 10 | namespace se2clam{ 11 | 12 | 13 | struct Se2{ 14 | float x; 15 | float y; 16 | float theta; 17 | Se2(); 18 | Se2(float _x, float _y ,float _theta); 19 | ~Se2(); 20 | Se2 operator -(const Se2& tominus); 21 | Se2 operator +(const Se2& toadd); 22 | cv::Mat toCvSE3(); 23 | }; 24 | 25 | class WorkTimer 26 | { 27 | private: 28 | int64 tickBegin, tickEnd; 29 | public: 30 | WorkTimer(){} 31 | ~WorkTimer(){} 32 | double time; 33 | void start(){ 34 | tickBegin = cv::getTickCount(); 35 | } 36 | 37 | void stop(){ 38 | tickEnd = cv::getTickCount(); 39 | time = (double)(tickEnd- tickBegin) / ((double)cv::getTickFrequency()) * 1000.; 40 | } 41 | }; 42 | 43 | 44 | class Config{ 45 | public: 46 | static std::string DataPath; 47 | static int ImgIndex; 48 | static int ImgIndexLocalSt; 49 | static cv::Size ImgSize; 50 | static cv::Mat bTc; // camera extrinsic 51 | static cv::Mat cTb; // inv of bTc 52 | static cv::Mat Kcam; // camera intrinsic 53 | static float fxCam, fyCam; 54 | static cv::Mat Dcam; // camera distortion 55 | 56 | static float UPPER_DEPTH; 57 | static float LOWER_DEPTH; 58 | 59 | static int NUM_FILTER_LAST_SEVERAL_MU; 60 | static int FILTER_CONVERGE_CONTINUE_COUNT; 61 | static float DEPTH_FILTER_THRESHOLD; 62 | 63 | static float ScaleFactor; // scalefactor in detecting features 64 | static int MaxLevel; // level number of pyramid in detecting features 65 | static int MaxFtrNumber; // max feature number to detect 66 | static float FEATURE_SIGMA; 67 | 68 | static float ODO_X_UNCERTAIN, ODO_Y_UNCERTAIN, ODO_T_UNCERTAIN; 69 | static float ODO_X_NOISE, ODO_Y_NOISE, ODO_T_NOISE; 70 | 71 | static float PLANEMOTION_Z_INFO; 72 | static float PLANEMOTION_XROT_INFO; 73 | static float PLANEMOTION_YROT_INFO; 74 | 75 | static int LOCAL_FRAMES_NUM; 76 | static float TH_HUBER; 77 | static int LOCAL_ITER; 78 | static bool LOCAL_VERBOSE; 79 | static int GLOBAL_ITER; 80 | static bool GLOBAL_VERBOSE; 81 | 82 | static bool LOCAL_PRINT; 83 | static bool GLOBAL_PRINT; 84 | 85 | static int FPS; 86 | static cv::Mat PrjMtrxEye; 87 | 88 | static bool USE_PREV_MAP; 89 | static bool LOCALIZATION_ONLY; 90 | static bool SAVE_NEW_MAP; 91 | static std::string READ_MAP_FILE_NAME; 92 | static std::string WRITE_MAP_FILE_NAME; 93 | static std::string READ_MAP_FILE_PATH; 94 | static std::string WRITE_MAP_FILE_PATH; 95 | 96 | static std::string WRITE_TRAJ_FILE_NAME; 97 | static std::string WRITE_TRAJ_FILE_PATH; 98 | 99 | static int MAPPUB_SCALE_RATIO; 100 | 101 | static int GM_VCL_NUM_MIN_MATCH_MP; 102 | static int GM_VCL_NUM_MIN_MATCH_KP; 103 | static double GM_VCL_RATIO_MIN_MATCH_MP; 104 | 105 | static int GM_DCL_MIN_KFID_OFFSET; 106 | static double GM_DCL_MIN_SCORE_BEST; 107 | 108 | static void readConfig(const std::string& path); 109 | static bool acceptDepth(float depth); 110 | 111 | }; 112 | 113 | }//namespace se2clam 114 | 115 | #endif // CONFIG_H 116 | -------------------------------------------------------------------------------- /include/se2clam/Frame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef FRAME_H 6 | #define FRAME_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "Config.h" 12 | #include "ORBextractor.h" 13 | 14 | namespace se2clam { 15 | 16 | const int FRAME_GRID_ROWS = 48; 17 | const int FRAME_GRID_COLS = 64; 18 | 19 | class KeyFrame; 20 | class Frame 21 | { 22 | public: 23 | Frame(); 24 | Frame(const cv::Mat &im, const Se2& odo, ORBextractor* extractor, const cv::Mat &K, const cv::Mat &distCoef); 25 | 26 | Frame(const Frame& f); 27 | Frame& operator=(const Frame& f); 28 | ~Frame(); 29 | 30 | float getTime() const; 31 | void setTime(float time); 32 | 33 | void undistortKeyPoints(const cv::Mat &K, const cv::Mat &D); 34 | void computeBoundUn(const cv::Mat &K, const cv::Mat &D); 35 | 36 | // Image Info 37 | cv::Mat img; 38 | static float minXUn; 39 | static float minYUn; 40 | static float maxXUn; 41 | static float maxYUn; 42 | bool inImgBound(cv::Point2f pt); 43 | ORBextractor* mpORBExtractor; 44 | std::vector keyPoints; 45 | std::vector keyPointsUn; 46 | cv::Mat descriptors; 47 | int N; 48 | std::vector GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1) const; 49 | 50 | // Scale Pyramid Info 51 | int mnScaleLevels; 52 | float mfScaleFactor; 53 | std::vector mvScaleFactors; 54 | std::vector mvLevelSigma2; 55 | std::vector mvInvLevelSigma2; 56 | 57 | static bool mbInitialComputations; 58 | 59 | // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints 60 | static float mfGridElementWidthInv; 61 | static float mfGridElementHeightInv; 62 | std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]; 63 | 64 | // Compute the cell of a keypoint (return false if outside the grid) 65 | bool PosInGrid(cv::KeyPoint &kp, int &posX, int &posY); 66 | 67 | 68 | // pose info: pose to ref KF, pose to World, odometry raw. 69 | cv::Mat Tcr; 70 | cv::Mat Tcw; 71 | Se2 mOdo; 72 | 73 | int id; 74 | static int nextId; 75 | 76 | //! Multi-thread processing 77 | std::mutex mMutexImg; 78 | void copyImgTo(cv::Mat & imgRet); 79 | 80 | std::mutex mMutexDes; 81 | void copyDesTo(cv::Mat & desRet); 82 | 83 | protected: 84 | float mTime; 85 | }; 86 | 87 | typedef std::shared_ptr PtrFrame; 88 | 89 | } // namespace se2clam 90 | #endif // FRAME_H 91 | -------------------------------------------------------------------------------- /include/se2clam/FramePublish.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef FRAMEPUBLISH_H 6 | #define FRAMEPUBLISH_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace se2clam { 13 | 14 | class Track; 15 | class GlobalMapper; 16 | class Localizer; 17 | 18 | class FramePublish{ 19 | public: 20 | 21 | FramePublish(); 22 | FramePublish(Track* pTR, GlobalMapper* pGM); 23 | ~FramePublish(); 24 | 25 | void run(); 26 | 27 | cv::Mat drawMatchesInOneImg(const std::vector queryKeys, 28 | const cv::Mat &trainImg, const std::vector trainKeys, 29 | const std::vector &matches); 30 | 31 | cv::Mat drawKeys(const std::vector keys, const cv::Mat &mImg, 32 | std::vector matched); 33 | cv::Mat drawFrame(); 34 | 35 | void setLocalizer(Localizer* localizer); 36 | 37 | bool mbIsLocalize; 38 | Localizer* mpLocalizer; 39 | 40 | private: 41 | 42 | Track* mpTrack; 43 | GlobalMapper* mpGM; 44 | 45 | std::vector kp, kpRef; 46 | std::vector matches; 47 | 48 | cv::Mat mImg, mImgRef; 49 | cv::Mat mImgOut; 50 | 51 | }; 52 | 53 | 54 | } // namespace se2clam 55 | 56 | #endif // FRAMEPUBLISH_H 57 | -------------------------------------------------------------------------------- /include/se2clam/GlobalMapper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef GLOBALMAPPER_H 6 | #define GLOBALMAPPER_H 7 | 8 | #include 9 | #include "Map.h" 10 | #include "KeyFrame.h" 11 | #include "LocalMapper.h" 12 | #include "Track.h" 13 | 14 | #include "optimizer.h" 15 | #include "converter.h" 16 | #include "sparsifier.h" 17 | 18 | #include "Config.h" 19 | 20 | #include "ORBVocabulary.h" 21 | #include "ORBmatcher.h" 22 | 23 | namespace se2clam { 24 | 25 | class Map; 26 | class LocalMapper; 27 | 28 | class GlobalMapper{ 29 | public: 30 | GlobalMapper(); 31 | 32 | void run(); 33 | void setLocalMapper(LocalMapper* pLocalMapper); 34 | void setUpdated(bool val); 35 | void setMap(Map* pMap); 36 | 37 | bool CheckGMReady(); 38 | 39 | // Feature Constraint Graph Functions ... 40 | 41 | // Update feature constraint graph, on KFs pairs given by LocalMapper 42 | // void UpdateFeatGraph(); 43 | void UpdataFeatGraph(vector> & _vKFPairs); 44 | 45 | // Set KF pair waiting for feature constraint generation, called by localmapper 46 | void SetKFPairFeatEdge(PtrKeyFrame _pKFFrom, PtrKeyFrame _pKFTo); 47 | 48 | // Create single feature constraint between 2 KFs 49 | static int CreateFeatEdge(PtrKeyFrame _pKFFrom, PtrKeyFrame _pKFTo, SE3Constraint & SE3CnstrOutput); 50 | static int CreateFeatEdge(PtrKeyFrame _pKFFrom, PtrKeyFrame _pKFTo, map & mapMatch, 51 | SE3Constraint & SE3CnstrOutput); 52 | 53 | // Do local optimization with chosen KFs and MPs 54 | static void OptKFPair(const vector & _vPtrKFs, const vector & _vPtrMPs, 55 | vector > & _vSe3KFs, vector > & _vPt3MPs); 56 | static void OptKFPairMatch(PtrKeyFrame _pKF1, PtrKeyFrame _pKF2, map & mapMatch, 57 | vector > & _vSe3KFs, vector > & _vPt3MPs); 58 | 59 | // generate MeasSE3XYZ measurement vector 60 | static void CreateVecMeasSE3XYZ(const vector _vpKFs, const vector _vpMPs, 61 | vector > & vMeas); 62 | 63 | vector> SelectKFPairFeat(const PtrKeyFrame _pKF); 64 | 65 | static set GetAllConnectedKFs(const PtrKeyFrame _pKF, set _sKFSelected = set()); 66 | static set GetAllConnectedKFs_nLayers(const PtrKeyFrame _pKF, int numLayers = 10, set _sKFSelected = set()); 67 | 68 | 69 | 70 | 71 | // Loop Closing ... 72 | ORBVocabulary* mpORBVoc; 73 | PtrKeyFrame mpLastKFLoopDetect; 74 | void setORBVoc(ORBVocabulary* pORBVoc); 75 | void ComputeBowVecAll(); 76 | bool DetectLoopClose(); 77 | bool VerifyLoopClose(map & _mapMatchMP, map & _mapMatchAll, map & _mapMatchRaw); 78 | void GlobalBA(); 79 | 80 | void DrawMatch(const map & mapiMatch); 81 | void DrawMatch(const vector & viMatch); 82 | 83 | void RemoveMatchOutlierRansac(PtrKeyFrame _pKFCurrent, PtrKeyFrame _pKFLoop, 84 | map & mapiMatch); 85 | void RemoveKPMatch(PtrKeyFrame _pKFCurrent, PtrKeyFrame _pKFLoop, 86 | map & mapiMatch); 87 | 88 | 89 | // DEBUG Functions ... 90 | // Print SE3Quat 91 | void PrintSE3(const g2o::SE3Quat se3); 92 | void PrintOptInfo(const SlamOptimizer & _optimizer); 93 | void PrintOptInfo(const vector & vpEdgeOdo, 94 | const vector & vpEdgeFeat, 95 | const vector & vpEdgePlane, 96 | double threshChi2 = 30.0, bool bPrintMatInfo = false); 97 | 98 | // FramePublish Related ... 99 | cv::Mat mImgLoop; 100 | cv::Mat mImgCurr; 101 | cv::Mat mImgMatch; 102 | 103 | bool mbExit; 104 | 105 | void setBusy(bool v); 106 | void waitIfBusy(); 107 | 108 | void requestFinish(); 109 | bool isFinished(); 110 | 111 | protected: 112 | Map* mpMap; 113 | LocalMapper* mpLocalMapper; 114 | 115 | PtrKeyFrame mpKFCurr; 116 | PtrKeyFrame mpKFLoop; 117 | std::deque > mdeqPairKFs; 118 | 119 | bool mbUpdated; 120 | bool mbNewKF; 121 | 122 | bool mbGlobalBALastLoop; 123 | 124 | bool mbIsBusy; 125 | std::condition_variable mcIsBusy; 126 | std::mutex mMutexBusy; 127 | 128 | bool checkFinish(); 129 | void setFinish(); 130 | bool mbFinishRequested; 131 | bool mbFinished; 132 | std::mutex mMutexFinish; 133 | 134 | }; 135 | 136 | } 137 | 138 | #endif // GLOBALMAPPER_H 139 | -------------------------------------------------------------------------------- /include/se2clam/KeyFrame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef KEYFRAME_H 6 | #define KEYFRAME_H 7 | 8 | 9 | #include 10 | #include "Frame.h" 11 | #include "Config.h" 12 | #include "converter.h" 13 | 14 | #include "Thirdparty/DBoW2/DBoW2/BowVector.h" 15 | #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" 16 | #include "ORBVocabulary.h" 17 | 18 | namespace se2clam{ 19 | 20 | struct SE3Constraint{ 21 | public: 22 | cv::Mat measure; 23 | cv::Mat info; 24 | SE3Constraint(){} 25 | SE3Constraint(const cv::Mat& _mea, const cv::Mat& _info){ 26 | _mea.copyTo(measure); 27 | _info.copyTo(info); 28 | } 29 | ~SE3Constraint(){} 30 | }; 31 | 32 | typedef shared_ptr PtrSE3Cstrt; 33 | 34 | class MapPoint; 35 | typedef shared_ptr PtrMapPoint; 36 | 37 | class KeyFrame: public Frame{ 38 | public: 39 | KeyFrame(); 40 | KeyFrame(const Frame &frame); 41 | ~KeyFrame(); 42 | 43 | cv::Mat getPose(); 44 | void setPose(const cv::Mat& _Tcw); 45 | 46 | int mIdKF; 47 | static int mNextIdKF; 48 | 49 | struct IdLessThan{ 50 | bool operator() (const shared_ptr& lhs, const shared_ptr& rhs) const{ 51 | return lhs->mIdKF < rhs->mIdKF; 52 | } 53 | }; 54 | 55 | 56 | void setNull(const shared_ptr &pThis); 57 | bool isNull(); 58 | 59 | std::set > getAllCovisibleKFs(); 60 | void eraseCovisibleKF(const shared_ptr pKF); 61 | void addCovisibleKF(const shared_ptr pKF); 62 | 63 | //! Functions for observation operations 64 | std::set getAllObsMPs(bool checkParallax = true); 65 | 66 | void addObservation(PtrMapPoint pMP, int idx); 67 | 68 | // Return all observations as a std::map 69 | std::map getObservations(); 70 | 71 | // Count how many observed MP 72 | int getSizeObsMP(); 73 | 74 | void eraseObservation(const PtrMapPoint pMP); 75 | void eraseObservation(int idx); 76 | 77 | // Whether a MP is observed by this KF 78 | bool hasObservation(const PtrMapPoint& pMP); 79 | 80 | // Whether the index in image KeyPoints corresponds to an observed MP 81 | bool hasObservation(int idx); 82 | 83 | // Get an observed MP by an index 84 | PtrMapPoint getObservation(int id); 85 | 86 | // Get the corresponding index of an observed MP 87 | int getFtrIdx(const PtrMapPoint& pMP); 88 | 89 | // Set a new MP in location index (used in MP merge) 90 | void setObservation(const PtrMapPoint& pMP, int idx); 91 | 92 | vector mViewMPs; 93 | vector > mViewMPsInfo; 94 | void setViewMP(cv::Point3f pt3f, int idx, Eigen::Matrix3d info); 95 | 96 | // KeyFrame contraints: From this or To this 97 | std::map< shared_ptr, SE3Constraint > mFtrMeasureFrom; 98 | std::map< shared_ptr, SE3Constraint > mFtrMeasureTo; 99 | std::pair< shared_ptr, SE3Constraint > mOdoMeasureFrom; 100 | std::pair< shared_ptr, SE3Constraint > mOdoMeasureTo; 101 | 102 | void addFtrMeasureFrom(shared_ptr pKF, const cv::Mat& _mea, const cv::Mat& _info); 103 | void addFtrMeasureTo(shared_ptr pKF, const cv::Mat& _mea, const cv::Mat& _info); 104 | void eraseFtrMeasureFrom(shared_ptr pKF); 105 | void eraseFtrMeasureTo(shared_ptr pKF); 106 | 107 | void setOdoMeasureFrom(shared_ptr pKF, const cv::Mat& _mea, const cv::Mat& _info); 108 | void setOdoMeasureTo(shared_ptr pKF, const cv::Mat& _mea, const cv::Mat& _info); 109 | 110 | // ORB BoW by THB: 111 | void ComputeBoW(ORBVocabulary* _pVoc); 112 | DBoW2::FeatureVector GetFeatureVector(); 113 | DBoW2::BowVector GetBowVector(); 114 | DBoW2::BowVector mBowVec; 115 | DBoW2::FeatureVector mFeatVec; 116 | bool mbBowVecExist; 117 | vector GetMapPointMatches(); 118 | 119 | // Copy Image 120 | // void copyImgTo(cv::Mat & imgRet); 121 | 122 | protected: 123 | 124 | std::map mObservations; 125 | std::map mDualObservations; 126 | 127 | bool mbNull; 128 | std::set > mCovisibleKFs; 129 | std::mutex mMutexPose; 130 | std::mutex mMutexObs; 131 | // std::mutex mMutexImg; 132 | }; 133 | 134 | typedef shared_ptr PtrKeyFrame; 135 | 136 | 137 | 138 | } // namespace se2clam 139 | #endif 140 | -------------------------------------------------------------------------------- /include/se2clam/LocalMapper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef LOCALMAPPER_H 6 | #define LOCALMAPPER_H 7 | 8 | #include "Map.h" 9 | #include "optimizer.h" 10 | namespace se2clam{ 11 | 12 | //#define TIME_TO_LOG_LOCAL_BA 13 | 14 | class GlobalMapper; 15 | 16 | class LocalMapper{ 17 | public: 18 | LocalMapper(); 19 | 20 | void run(); 21 | 22 | void setMap(Map *pMap); 23 | 24 | void setGlobalMapper(GlobalMapper* pGlobalMapper); 25 | 26 | void addNewKF(PtrKeyFrame &pKF, const std::vector& localMPs, const std::vector &vMatched12, const std::vector& vbGoodPrl); 27 | 28 | void findCorrespd(const std::vector &vMatched12, const std::vector &localMPs, const std::vector& vbGoodPrl); 29 | 30 | void removeOutlierChi2(); 31 | 32 | void localBA(); 33 | 34 | void setAbortBA(); 35 | 36 | bool acceptNewKF(); 37 | 38 | void setGlobalBABegin(bool value); 39 | 40 | void printOptInfo(const SlamOptimizer & _optimizer); // For debugging by hbtang 41 | bool mbPrintDebugInfo; 42 | 43 | 44 | void requestFinish(); 45 | bool isFinished(); 46 | 47 | std::mutex mutexMapper; 48 | 49 | void updateLocalGraphInMap(); 50 | 51 | void pruneRedundantKfInMap(); 52 | 53 | 54 | protected: 55 | Map* mpMap; 56 | GlobalMapper* mpGlobalMapper; 57 | PtrKeyFrame mNewKF; 58 | 59 | bool mbUpdated; 60 | bool mbAbortBA; 61 | bool mbAcceptNewKF; 62 | bool mbGlobalBABegin; 63 | 64 | mutex mMutexLocalGraph; 65 | 66 | bool checkFinish(); 67 | void setFinish(); 68 | bool mbFinishRequested; 69 | bool mbFinished; 70 | std::mutex mMutexFinish; 71 | 72 | 73 | 74 | }; 75 | 76 | } 77 | 78 | 79 | #endif // LOCALMAPPER_H 80 | -------------------------------------------------------------------------------- /include/se2clam/Localizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef LOCALIZE_H 6 | #define LOCALIZE_H 7 | 8 | #include "Config.h" 9 | #include "Frame.h" 10 | #include "KeyFrame.h" 11 | #include "Map.h" 12 | #include "Sensors.h" 13 | #include "sugarCV.h" 14 | 15 | namespace se2clam { 16 | 17 | class Localizer { 18 | public: 19 | //! Functions 20 | Localizer(); 21 | ~Localizer(); 22 | 23 | // Main 24 | void run(); 25 | 26 | // Initialization 27 | void setMap(Map *pMap); 28 | void setORBVoc(ORBVocabulary* pORBVoc); 29 | void setSensors(Sensors* pSensors); 30 | 31 | void ReadFrameInfo(const cv::Mat &img, const Se2& odo); 32 | 33 | void MatchLastFrame(); 34 | void MatchLocalMap(); 35 | 36 | // Loop closing 37 | bool DetectLoopClose(); 38 | bool VerifyLoopClose(std::map & mapMatchMP, std::map & mapMatchAll, std::map & mapMatchRaw); 39 | void MatchLoopClose(std::map mapMatchGood); 40 | 41 | void DoLocalBA(); 42 | void DetectIfLost(); 43 | 44 | // Local map 45 | void UpdateLocalMap(int searchLevel = 3); 46 | void UpdateLocalMapTrack(); 47 | void ResetLocalMap(); 48 | 49 | // Subfunctions 50 | void RemoveMatchOutlierRansac(PtrKeyFrame pKFCurr, PtrKeyFrame pKFLoop, std::map & mapMatch); 51 | void ComputeBowVecAll(); 52 | std::vector GetLocalKFs(); 53 | std::vector GetLocalMPs(); 54 | 55 | // IO 56 | void UpdatePoseCurr(); 57 | void DrawImgMatch(const std::map & mapMatch); 58 | void DrawImgCurr(); 59 | 60 | 61 | void UpdateCovisKFCurr(); 62 | int FindCommonMPs(const PtrKeyFrame pKF1, const PtrKeyFrame pKF2, std::set& spMPs); 63 | 64 | // DEBUG 65 | void Test(int a = 1, int b = 2); 66 | void WriteTrajFile(std::ofstream & file); 67 | 68 | void requestFinish(); 69 | bool isFinished(); 70 | 71 | 72 | 73 | public: 74 | //! Variables 75 | bool mbIsTracked; 76 | 77 | Map* mpMap; 78 | ORBextractor* mpORBextractor; 79 | ORBVocabulary* mpORBVoc; 80 | 81 | Frame mFrameCurr; 82 | PtrKeyFrame mpKFCurr; 83 | PtrKeyFrame mpKFCurrRefined; 84 | 85 | Frame mFrameRef; 86 | PtrKeyFrame mpKFRef; 87 | PtrKeyFrame mpKFLoop; 88 | 89 | std::set mspMPLocal; 90 | std::set mspKFLocal; 91 | 92 | cv::Mat mImgCurr; 93 | cv::Mat mImgLoop; 94 | cv::Mat mImgMatch; 95 | 96 | mutex mMutexImg; 97 | mutex mMutexMPLocal; 98 | mutex mMutexKFLocal; 99 | mutex mMutexLocalMap; 100 | 101 | protected: 102 | bool checkFinish(); 103 | void setFinish(); 104 | bool mbFinishRequested; 105 | bool mbFinished; 106 | std::mutex mMutexFinish; 107 | 108 | Sensors* mpSensors; 109 | }; 110 | 111 | 112 | 113 | 114 | 115 | } 116 | 117 | 118 | #endif // LOCALIZE_H 119 | -------------------------------------------------------------------------------- /include/se2clam/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef MAP_H 6 | #define MAP_H 7 | 8 | #include "KeyFrame.h" 9 | #include "MapPoint.h" 10 | #include "Config.h" 11 | #include "optimizer.h" 12 | #include 13 | #include 14 | 15 | namespace se2clam { 16 | 17 | class LocalMapper; 18 | 19 | class Map{ 20 | 21 | public: 22 | Map(); 23 | ~Map(); 24 | 25 | void insertKF(const PtrKeyFrame& pkf); 26 | void insertMP(const PtrMapPoint& pmp); 27 | 28 | void eraseKF(const PtrKeyFrame& pKF); 29 | void eraseMP(const PtrMapPoint& pMP); 30 | 31 | std::vector getAllKF(); 32 | std::vector getAllMP(); 33 | size_t countKFs(); 34 | size_t countMPs(); 35 | 36 | void clear(); 37 | bool empty(); 38 | 39 | PtrKeyFrame getCurrentKF(); 40 | void setCurrentKF(const PtrKeyFrame &pKF); 41 | 42 | void setCurrentFramePose(const cv::Mat& pose); 43 | cv::Mat getCurrentFramePose(); 44 | 45 | cv::SparseMat mFtrBasedGraph; 46 | cv::SparseMat mOdoBasedGraph; 47 | std::unordered_map mFtrBasedEdges; 48 | std::unordered_map mOdoBasedEdges; 49 | std::vector mIdxFtrBased; 50 | std::vector mIdxOdoBased; 51 | 52 | void mergeMP(PtrMapPoint& toKeep, PtrMapPoint& toDelete); 53 | 54 | 55 | bool locked(); 56 | void lock(); 57 | void unlock(); 58 | 59 | static cv::Point2f compareViewMPs(const PtrKeyFrame& pKF1, const PtrKeyFrame& pKF2, std::set& spMPs); 60 | 61 | static double compareViewMPs(const PtrKeyFrame & pKF, const set & vpKFs, std::set & vpMPs, int k = 2); 62 | 63 | static bool checkAssociationErr(const PtrKeyFrame& pKF, const PtrMapPoint& pMP); 64 | 65 | 66 | //! For LocalMapper 67 | void setLocalMapper(LocalMapper* pLocalMapper); 68 | 69 | void updateLocalGraph(); 70 | 71 | bool pruneRedundantKF(); 72 | 73 | void loadLocalGraph(SlamOptimizer& optimizer, std::vector< std::vector > &vpEdgesAll, std::vector< std::vector >& vnAllIdx); 74 | 75 | void loadLocalGraphOnlyBa(SlamOptimizer& optimizer, std::vector< std::vector > &vpEdgesAll, std::vector< std::vector >& vnAllIdx); 76 | 77 | int removeLocalOutlierMP(const vector > &vnOutlierIdxAll); 78 | 79 | void optimizeLocalGraph(SlamOptimizer& optimizer); 80 | 81 | void updateCovisibility(PtrKeyFrame& pNewKF); 82 | 83 | std::vector getLocalKFs(); 84 | std::vector getLocalMPs(); 85 | std::vector getRefKFs(); 86 | 87 | int countLocalKFs(); 88 | int countLocalMPs(); 89 | 90 | 91 | //! For GlobalMapper 92 | void mergeLoopClose(const std::map& mapMatchMP, PtrKeyFrame& pKFCurr, PtrKeyFrame& pKFLoop); 93 | 94 | //! Set KF pair waiting for feature constraint generation, called by localmapper 95 | std::vector> SelectKFPairFeat(const PtrKeyFrame &_pKF); 96 | 97 | //! Update feature constraint graph, on KFs pairs given by LocalMapper 98 | bool UpdateFeatGraph(const PtrKeyFrame &_pKF); 99 | 100 | 101 | protected: 102 | 103 | PtrKeyFrame mCurrentKF; 104 | 105 | bool isEmpty; 106 | 107 | //! Global Map 108 | std::set mMPs; 109 | std::set mKFs; 110 | 111 | //! Local Map 112 | std::vector mLocalGraphMPs; 113 | std::vector mLocalGraphKFs; 114 | std::vector mRefKFs; 115 | LocalMapper* mpLocalMapper; 116 | 117 | cv::Mat mCurrentFramePose; 118 | 119 | std::mutex mMutexGraph; 120 | std::mutex mMutexLocalGraph; 121 | std::mutex mMutexCurrentKF; 122 | std::mutex mMutexCurrentFrame; 123 | 124 | }; //class Map 125 | 126 | }// namespace se2clam 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /include/se2clam/MapPoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef MAPPOINT_H 6 | #define MAPPOINT_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace se2clam{ 16 | class KeyFrame; 17 | 18 | typedef std::shared_ptr PtrKeyFrame; 19 | 20 | class MapPoint 21 | { 22 | public: 23 | MapPoint(); 24 | MapPoint(cv::Point3f pos, bool goodPrl); 25 | ~MapPoint(); 26 | 27 | std::set getObservations(); 28 | 29 | bool hasObservation(const PtrKeyFrame& pKF); 30 | 31 | void eraseObservation(const PtrKeyFrame& pKF); 32 | 33 | int countObservation(); 34 | 35 | // Do pKF.setViewMP() before use this 36 | void addObservation(const PtrKeyFrame &pKF, int idx); 37 | 38 | int getOctave(const PtrKeyFrame pKF); 39 | 40 | bool isGoodPrl(); 41 | void setGoodPrl(bool value); 42 | 43 | bool isNull(); 44 | void setNull(const std::shared_ptr &pThis); 45 | 46 | cv::Point3f getPos(); 47 | void setPos(const cv::Point3f& pt3f); 48 | 49 | float getInvLevelSigma2(const PtrKeyFrame &pKF); 50 | 51 | // The descriptor with least median distance to the rest 52 | cv::Mat mMainDescriptor; 53 | PtrKeyFrame mMainKF; 54 | int mMainOctave; 55 | float mLevelScaleFactor; 56 | cv::Point2f getMainMeasure(); 57 | void updateMainKFandDescriptor(); 58 | 59 | bool acceptNewObserve(cv::Point3f posKF, const cv::KeyPoint kp); 60 | 61 | void updateParallax(const PtrKeyFrame& pKF); 62 | 63 | void updateMeasureInKFs(); 64 | 65 | int mId; 66 | static int mNextId; 67 | 68 | struct IdLessThan{ 69 | bool operator() (const std::shared_ptr& lhs, const std::shared_ptr& rhs) const{ 70 | return lhs->mId < rhs->mId; 71 | } 72 | }; 73 | 74 | 75 | void revisitFailCount(); 76 | 77 | // This MP would be replaced and abandoned later by 78 | void mergedInto(const std::shared_ptr& pMP); 79 | 80 | int getFtrIdx(PtrKeyFrame pKF); 81 | 82 | protected: 83 | std::map mObservations; 84 | 85 | cv::Point3f mPos; 86 | 87 | void setNull(); 88 | bool mbNull; 89 | bool mbGoodParallax; 90 | 91 | float mMinDist; 92 | float mMaxDist; 93 | 94 | // Mean view direction 95 | cv::Point3f mNormalVector; 96 | 97 | std::mutex mMutexPos; 98 | std::mutex mMutexObs; 99 | 100 | }; 101 | 102 | typedef std::shared_ptr PtrMapPoint; 103 | 104 | 105 | } //namespace se2clam 106 | 107 | #endif // MAPPOINT_H 108 | -------------------------------------------------------------------------------- /include/se2clam/MapPublish.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef MAPPUBLISH_H 6 | #define MAPPUBLISH_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "LocalMapper.h" 12 | #include "Localizer.h" 13 | #include "FramePublish.h" 14 | 15 | 16 | namespace se2clam { 17 | 18 | class Map; 19 | 20 | class MapPublish{ 21 | public: 22 | MapPublish(Map* pMap); 23 | ~MapPublish(); 24 | Map* mpMap; 25 | LocalMapper* mpLocalMapper; 26 | Localizer* mpLocalize; 27 | FramePublish* mpFramePub; 28 | 29 | void run(); 30 | 31 | 32 | void setFramePub(FramePublish* pFP); 33 | void setMap(Map* pMap) { mpMap = pMap; } 34 | void setLocalMapper(LocalMapper* pLocal) { mpLocalMapper = pLocal; } 35 | void setLocalizer(Localizer* pLocalize) { mpLocalize = pLocalize; } 36 | 37 | void PublishMapPoints(); 38 | void PublishKeyFrames(); 39 | void PublishCameraCurr(const cv::Mat &Twc); 40 | 41 | private: 42 | 43 | ros::NodeHandle nh; 44 | ros::Publisher publisher; 45 | tf::TransformBroadcaster tfb; 46 | 47 | visualization_msgs::Marker mMPsNeg; 48 | visualization_msgs::Marker mMPsAct; 49 | visualization_msgs::Marker mMPsNow; 50 | 51 | visualization_msgs::Marker mKFsNeg; 52 | visualization_msgs::Marker mKFsAct; 53 | visualization_msgs::Marker mKFNow; 54 | 55 | visualization_msgs::Marker mCovisGraph; 56 | visualization_msgs::Marker mFeatGraph; 57 | visualization_msgs::Marker mOdoGraph; 58 | visualization_msgs::Marker mMST; 59 | 60 | 61 | float mPointSize; 62 | float mCameraSize; 63 | float mScaleRatio; 64 | 65 | bool CheckFinish(); 66 | void SetFinish(); 67 | bool mbFinishRequested; 68 | bool mbFinished; 69 | std::mutex mMutexFinish; 70 | 71 | public: 72 | bool mbIsLocalize; 73 | 74 | void RequestFinish(); 75 | bool isFinished(); 76 | 77 | };// class MapPublisher 78 | 79 | } // namespace se2clam 80 | 81 | #endif // MAPPUBLISH_H 82 | -------------------------------------------------------------------------------- /include/se2clam/MapStorage.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef MAPSTORAGE_H 6 | #define MAPSTORAGE_H 7 | 8 | #include "Map.h" 9 | 10 | 11 | //! The storage file structure would be like: 12 | 13 | /** 14 | - /somewhere/set/as/MapPath/ 15 | | 16 | |---[MapFileName] 17 | |---0.bmp 18 | |---1.bmp 19 | |---2.bmp 20 | |---... 21 | **/ 22 | 23 | namespace se2clam { 24 | 25 | class MapStorage 26 | { 27 | 28 | public: 29 | 30 | MapStorage(); 31 | 32 | void setMap(Map* pMap); 33 | 34 | void setFilePath(const string path, const string file); 35 | 36 | // Save map to file 37 | void saveMap(); 38 | 39 | // Load map from file 40 | void loadMap(); 41 | 42 | void clearData(); 43 | 44 | 45 | protected: 46 | 47 | void sortKeyFrames(); 48 | 49 | void sortMapPoints(); 50 | 51 | void saveKeyFrames(); 52 | 53 | void saveMapPoints(); 54 | 55 | void saveObservations(); 56 | 57 | void saveCovisibilityGraph(); 58 | 59 | void saveOdoGraph(); 60 | 61 | void saveFtrGraph(); 62 | 63 | void loadKeyFrames(); 64 | 65 | void loadMapPoints(); 66 | 67 | void loadObservations(); 68 | 69 | void loadCovisibilityGraph(); 70 | 71 | void loadOdoGraph(); 72 | 73 | void loadFtrGraph(); 74 | 75 | void loadToMap(); 76 | 77 | Map* mpMap; 78 | 79 | string mMapPath; 80 | string mMapFile; 81 | 82 | vector mvKFs; 83 | 84 | vector mvMPs; 85 | 86 | cv::Mat_ mObservations; 87 | 88 | cv::Mat_ mCovisibilityGraph; 89 | 90 | vector mOdoNextId; 91 | 92 | }; 93 | 94 | }// namespace se2clam 95 | 96 | #endif // MAPSTORAGE_H 97 | -------------------------------------------------------------------------------- /include/se2clam/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | /** 6 | * This file is part of ORB-SLAM. 7 | * 8 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 9 | * For more information see 10 | * 11 | * ORB-SLAM is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * ORB-SLAM is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with ORB-SLAM. If not, see . 23 | */ 24 | 25 | 26 | #ifndef ORBVOCABULARY_H 27 | #define ORBVOCABULARY_H 28 | 29 | #include "Thirdparty/DBoW2/DBoW2/FORB.h" 30 | #include "Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 31 | 32 | namespace se2clam 33 | { 34 | 35 | typedef DBoW2::TemplatedVocabulary 36 | ORBVocabulary; 37 | 38 | } //namespace ORB_SLAM 39 | 40 | #endif // ORBVOCABULARY_H 41 | -------------------------------------------------------------------------------- /include/se2clam/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | /** 6 | * This file is part of ORB-SLAM. 7 | * 8 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 9 | * For more information see 10 | * 11 | * ORB-SLAM is free software: you can redistribute it and/or modify 12 | * it under the terms of the GNU General Public License as published by 13 | * the Free Software Foundation, either version 3 of the License, or 14 | * (at your option) any later version. 15 | * 16 | * ORB-SLAM is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with ORB-SLAM. If not, see . 23 | */ 24 | 25 | #ifndef ORBEXTRACTOR_H 26 | #define ORBEXTRACTOR_H 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace se2clam 34 | { 35 | 36 | class ORBextractor 37 | { 38 | public: 39 | 40 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 41 | 42 | ORBextractor(int nfeatures = 1000, float scaleFactor = 1.2f, int nlevels = 8, int scoreType=FAST_SCORE, int fastTh = 20); 43 | 44 | ~ORBextractor(){} 45 | 46 | // Compute the ORB features and descriptors on an image 47 | void operator()( cv::InputArray image, cv::InputArray mask, 48 | std::vector& keypoints, 49 | cv::OutputArray descriptors); 50 | 51 | int inline GetLevels(){ 52 | return nlevels;} 53 | 54 | float inline GetScaleFactor(){ 55 | return scaleFactor;} 56 | 57 | 58 | protected: 59 | 60 | void ComputePyramid(cv::Mat image, cv::Mat Mask=cv::Mat()); 61 | void ComputeKeyPoints(std::vector >& allKeypoints); 62 | 63 | std::vector pattern; 64 | 65 | int nfeatures; 66 | double scaleFactor; 67 | int nlevels; 68 | int scoreType; 69 | int fastTh; 70 | 71 | std::vector mnFeaturesPerLevel; 72 | 73 | std::vector umax; 74 | 75 | std::vector mvScaleFactor; 76 | std::vector mvInvScaleFactor; 77 | 78 | std::vector mvImagePyramid; 79 | std::vector mvMaskPyramid; 80 | 81 | }; 82 | 83 | } //namespace se2clam 84 | 85 | #endif 86 | 87 | -------------------------------------------------------------------------------- /include/se2clam/ORBmatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBMATCHER_H 23 | #define ORBMATCHER_H 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include"MapPoint.h" 30 | #include"KeyFrame.h" 31 | #include"Frame.h" 32 | 33 | namespace se2clam 34 | { 35 | 36 | class ORBmatcher 37 | { 38 | public: 39 | 40 | ORBmatcher(float nnratio=0.6, bool checkOri=true); 41 | 42 | // Computes the Hamming distance between two ORB descriptors 43 | static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 44 | 45 | 46 | // Search matches between MapPoints in a KeyFrame and ORB in a Frame. 47 | // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level) 48 | // Used in Relocalisation and Loop Detection 49 | int SearchByBoW(PtrKeyFrame pKF1, PtrKeyFrame pKF2, 50 | std::map &mapIdxMatches12, bool bIfMPOnly = true); 51 | // int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector &vpMapPointMatches); 52 | void ComputeThreeMaxima(std::vector* histo, const int L, int &ind1, int &ind2, int &ind3); 53 | 54 | 55 | static const int TH_LOW; 56 | static const int TH_HIGH; 57 | static const int HISTO_LENGTH; 58 | 59 | float mfNNratio; 60 | bool mbCheckOrientation; 61 | 62 | int MatchByWindow(const Frame& frame1, Frame& frame2, 63 | std::vector& vbPrevMatched, const int winSize, 64 | std::vector& vnMatches12, const int levelOffset = 1, 65 | const int minLevel = 0, const int maxLevel = 8); 66 | 67 | int MatchByProjection(PtrKeyFrame& pNewKF, std::vector& localMPs, const int winSize, const int levelOffset, 68 | std::vector& vMatchesIdxMP); 69 | 70 | 71 | float RadiusByViewingCos(const float &viewCos); 72 | 73 | 74 | }; 75 | 76 | }// namespace se2clam 77 | 78 | 79 | #endif // ORBMATCHER_H 80 | -------------------------------------------------------------------------------- /include/se2clam/OdoSLAM.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef ODOSLAM_H 6 | #define ODOSLAM_H 7 | 8 | #include "sugarCV.h" 9 | #include "Track.h" 10 | #include "LocalMapper.h" 11 | #include "GlobalMapper.h" 12 | #include "Map.h" 13 | #include "Config.h" 14 | #include "MapStorage.h" 15 | #include "FramePublish.h" 16 | #include "MapPublish.h" 17 | #include "Localizer.h" 18 | #include "Sensors.h" 19 | 20 | namespace se2clam { 21 | 22 | class OdoSLAM { 23 | 24 | public: 25 | OdoSLAM(); 26 | 27 | ~OdoSLAM(); 28 | 29 | void setDataPath(const char* strDataPath); 30 | 31 | void setVocFileBin(const char *strVoc); 32 | 33 | void start(); 34 | 35 | inline void receiveOdoData(double x_, double y_, double z_, double time_ = 0) 36 | { 37 | mpSensors->updateOdo(x_, y_, z_, time_); 38 | } 39 | 40 | inline void receiveImgData(const cv::Mat &img_, double time_ = 0) 41 | { 42 | mpSensors->updateImg(img_, time_); 43 | } 44 | 45 | 46 | void requestFinish(); 47 | 48 | void waitForFinish(); 49 | 50 | cv::Mat getCurrentVehiclePose(); 51 | 52 | cv::Mat getCurrentCameraPoseWC(); 53 | 54 | cv::Mat getCurrentCameraPoseCW(); 55 | 56 | 57 | bool ok(); 58 | 59 | private: 60 | 61 | Map* mpMap; 62 | LocalMapper* mpLocalMapper; 63 | GlobalMapper* mpGlobalMapper; 64 | FramePublish* mpFramePub; 65 | MapPublish* mpMapPub; 66 | Track* mpTrack; 67 | MapStorage* mpMapStorage; 68 | Localizer* mpLocalizer; 69 | Sensors* mpSensors; 70 | 71 | ORBVocabulary* mpVocabulary; 72 | 73 | bool mbFinishRequested; 74 | 75 | bool checkFinish(); 76 | 77 | bool mbFinished; 78 | 79 | void saveMap(); 80 | 81 | void sendRequestFinish(); 82 | 83 | void checkAllExit(); 84 | 85 | void clear(); 86 | 87 | static void wait(OdoSLAM* system); 88 | 89 | }; 90 | 91 | } 92 | 93 | 94 | #endif // ODOSLAM_H 95 | -------------------------------------------------------------------------------- /include/se2clam/Sensors.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef SENSORS_H 6 | #define SENSORS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace se2clam { 15 | 16 | class Sensors { 17 | 18 | public: 19 | 20 | Sensors(); 21 | 22 | ~Sensors(); 23 | 24 | bool update(); 25 | 26 | void setUpdated(bool val); 27 | 28 | void updateOdo(double x_, double y_, double theta_, double time_ = 0); 29 | 30 | void updateImg(const cv::Mat &img_, double time_ = 0); 31 | 32 | // After readData(), img_updatd and odo_updated would be set false 33 | void readData(cv::Point3f& dataOdo, cv::Mat& dataImg); 34 | 35 | void forceSetUpdate(bool val); 36 | 37 | protected: 38 | 39 | cv::Mat mImg; 40 | cv::Point3f mOdo; 41 | 42 | std::mutex mutex_odo; 43 | std::mutex mutex_img; 44 | 45 | std::atomic_bool odo_updated; 46 | std::atomic_bool img_updated; 47 | 48 | std::condition_variable cndvSensorUpdate; 49 | 50 | // reserve for sync 51 | double time_odo; 52 | double time_img; 53 | 54 | }; 55 | 56 | 57 | } 58 | 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /include/se2clam/Track.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef TRACK_H 6 | #define TRACK_H 7 | #include 8 | #include "Config.h" 9 | #include "Frame.h" 10 | #include "Sensors.h" 11 | 12 | namespace se2clam { 13 | 14 | class KeyFrame; 15 | typedef std::shared_ptr PtrKeyFrame; 16 | class Map; 17 | class LocalMapper; 18 | 19 | class Track{ 20 | public: 21 | Track(); 22 | ~Track(); 23 | 24 | void run(); 25 | 26 | void setMap(Map* pMap); 27 | 28 | void setLocalMapper(LocalMapper* pLocalMapper); 29 | 30 | void setSensors(Sensors* pSensors); 31 | 32 | static void calcOdoConstraintCam(const Se2 &dOdo, cv::Mat &cTc, g2o::Matrix6d &Info_se3); 33 | 34 | static void calcSE3toXYZInfo(cv::Point3f xyz1, const cv::Mat& Tcw1, const cv::Mat& Tcw2, Eigen::Matrix3d& info1, Eigen::Matrix3d& info2); 35 | 36 | // for frame publisher 37 | std::vector mMatchIdx; 38 | int copyForPub(std::vector& kp1, std::vector& kp2, cv::Mat& img1, cv::Mat& img2, std::vector &vMatches12); 39 | 40 | void requestFinish(); 41 | bool isFinished(); 42 | 43 | private: 44 | // only useful when odo time not sync with img time 45 | //float mTimeOdo; 46 | //float mTimeImg; 47 | 48 | static bool mbUseOdometry; 49 | Map* mpMap; 50 | LocalMapper* mpLocalMapper; 51 | 52 | Sensors* mpSensors; 53 | 54 | ORBextractor* mpORBextractor; 55 | 56 | std::vector mLocalMPs; 57 | int mnGoodPrl; // count number of mLocalMPs with good parallax 58 | std::vector mvbGoodPrl; 59 | 60 | int nMinFrames; 61 | int nMaxFrames; 62 | 63 | Frame mFrame; 64 | Frame mRefFrame; 65 | PtrKeyFrame mpKF; 66 | std::vector mPrevMatched; 67 | 68 | void mCreateFrame(const cv::Mat& img, const Se2& odo); 69 | void mTrack(const cv::Mat& img, const Se2& odo); 70 | void resetLocalTrack(); 71 | void updateFramePose(); 72 | int removeOutliers(const std::vector& kp1, const std::vector& kp2, std::vector& matches); 73 | bool needNewKF(int nTrackedOldMP, int nMatched); 74 | int doTriangulate(); 75 | 76 | std::mutex mMutexForPub; 77 | 78 | public: 79 | 80 | bool checkFinish(); 81 | void setFinish(); 82 | bool mbFinishRequested; 83 | bool mbFinished; 84 | std::mutex mMutexFinish; 85 | 86 | }; 87 | 88 | 89 | } // namespace se2clam 90 | 91 | #endif // TRACK_H 92 | -------------------------------------------------------------------------------- /include/se2clam/converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef CONVERTER_H 6 | #define CONVERTER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "Config.h" 12 | 13 | namespace se2clam{ 14 | 15 | //cv::Mat toT4x4(cv::Mat R, cv::Mat T); 16 | //cv::Mat toT4x4(float x, float y, float theta); 17 | 18 | Eigen::Vector2d toVector2d(const cv::Point2f &cvVector); 19 | g2o::Isometry3D toIsometry3D(const cv::Mat& T); 20 | cv::Mat toCvMat(const g2o::Isometry3D& t); 21 | cv::Point2f toCvPt2f(const Eigen::Vector2d& vec); 22 | cv::Point3f toCvPt3f(const Eigen::Vector3d& vec); 23 | g2o::Isometry3D toIsometry3D(const g2o::SE3Quat& se3quat); 24 | g2o::SE3Quat toSE3Quat(const g2o::Isometry3D& iso); 25 | 26 | // below from ORB_SLAM: https://github.com/raulmur/ORB_SLAM 27 | std::vector toDescriptorVector(const cv::Mat &Descriptors); 28 | g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 29 | cv::Mat toCvMat(const g2o::SE3Quat &SE3); 30 | cv::Mat toCvMat(const Eigen::Matrix &m); 31 | cv::Mat toCvMat(const Eigen::Matrix3d &m); 32 | cv::Mat toCvMat(const Eigen::Matrix &m); 33 | cv::Mat toCvMat6f(const g2o::Matrix6d& m); 34 | cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 35 | Eigen::Matrix toVector3d(const cv::Mat &cvVector); 36 | Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 37 | Eigen::Matrix toVector2d(const cv::Mat &cvVector); 38 | Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 39 | g2o::Matrix6d toMatrix6d(const cv::Mat& cvMat6d); 40 | std::vector toQuaternion(const cv::Mat &M); 41 | 42 | } // namespace se2clam 43 | #endif 44 | -------------------------------------------------------------------------------- /include/se2clam/optimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef OPTIMIZER_H 6 | #define OPTIMIZER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "Config.h" 21 | #include "KeyFrame.h" 22 | 23 | namespace se2clam{ 24 | 25 | typedef g2o::BlockSolverX SlamBlockSolver; 26 | typedef g2o::LinearSolverCholmod SlamLinearSolver; 27 | typedef g2o::OptimizationAlgorithmLevenberg SlamAlgorithm; 28 | typedef g2o::SparseOptimizer SlamOptimizer; 29 | typedef g2o::CameraParameters CamPara; 30 | 31 | inline Eigen::Quaterniond toQuaterniond(const Eigen::Vector3d &rot_vector) 32 | { 33 | double angle = rot_vector.norm(); 34 | if(angle <= 1e-14) 35 | return Eigen::Quaterniond(1, 0, 0, 0); 36 | else 37 | return Eigen::Quaterniond(Eigen::AngleAxisd(angle, rot_vector.normalized())); 38 | } 39 | 40 | inline Eigen::Vector3d toRotationVector(const Eigen::Quaterniond &q_) 41 | { 42 | Eigen::AngleAxisd angle_axis(q_); 43 | return angle_axis.angle() * angle_axis.axis(); 44 | } 45 | 46 | class G2O_TYPES_SBA_API EdgeSE3ExpmapPrior: public g2o::BaseUnaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap> { 47 | public: 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | EdgeSE3ExpmapPrior(); 50 | 51 | // Useless functions we don't care 52 | virtual bool read(std::istream &is); 53 | virtual bool write(std::ostream &os) const; 54 | 55 | void computeError(); 56 | 57 | void setMeasurement(const g2o::SE3Quat& m); 58 | 59 | virtual void linearizeOplus(); 60 | }; 61 | 62 | g2o::Matrix3D 63 | Jl(const g2o::Vector3D& v3d); 64 | 65 | g2o::Matrix3D 66 | invJl(const g2o::Vector3D& v3d); 67 | 68 | 69 | g2o::Matrix6d 70 | invJJl(const g2o::Vector6d& v6d); 71 | 72 | void 73 | initOptimizer(SlamOptimizer &opt, bool verbose=false); 74 | 75 | EdgeSE3ExpmapPrior* 76 | addPlaneMotionSE3Expmap(SlamOptimizer& opt, const g2o::SE3Quat &pose, int vId, const cv::Mat& extPara); 77 | 78 | 79 | CamPara* 80 | addCamPara(SlamOptimizer &opt, const cv::Mat& K, int id); 81 | 82 | void 83 | addVertexSE3Expmap(SlamOptimizer &opt, const g2o::SE3Quat& pose, int id, bool fixed=false); 84 | 85 | void 86 | addVertexSBAXYZ(SlamOptimizer &opt, const Eigen::Vector3d &xyz, int id, bool marginal=true, bool fixed=false); 87 | 88 | void 89 | addEdgeSE3Expmap(SlamOptimizer &opt, const g2o::SE3Quat& measure, int id0, int id1, const g2o::Matrix6d& info); 90 | 91 | g2o::EdgeProjectXYZ2UV* 92 | addEdgeXYZ2UV(SlamOptimizer &opt, const Eigen::Vector2d& measure, int id0, int id1, int paraId, const Eigen::Matrix2d &info, double thHuber); 93 | 94 | g2o::ParameterSE3Offset* 95 | addParaSE3Offset(SlamOptimizer &opt, const g2o::Isometry3D& se3offset, int id); 96 | 97 | void 98 | addVertexSE3(SlamOptimizer &opt, const g2o::Isometry3D &pose, int id, bool fixed=false); 99 | 100 | g2o::EdgeSE3Prior* 101 | addVertexSE3PlaneMotion(SlamOptimizer &opt, const g2o::Isometry3D &pose, int id, const cv::Mat& extPara, int paraSE3OffsetId, bool fixed=false); 102 | 103 | void 104 | addVertexXYZ(SlamOptimizer &opt, const g2o::Vector3D &xyz, int id, bool marginal=true); 105 | 106 | g2o::EdgeSE3* 107 | addEdgeSE3(SlamOptimizer &opt, const g2o::Isometry3D &measure, int id0, int id1, const g2o::Matrix6d& info); 108 | 109 | g2o::EdgeSE3PointXYZ* 110 | addEdgeSE3XYZ(SlamOptimizer &opt, const g2o::Vector3D& measure, int idse3, int idxyz, int paraSE3OffsetId, const g2o::Matrix3D &info, double thHuber); 111 | 112 | 113 | g2o::Isometry3D 114 | estimateVertexSE3(SlamOptimizer &opt, int id); 115 | 116 | Eigen::Vector3d 117 | estimateVertexXYZ(SlamOptimizer &opt, int id); 118 | 119 | g2o::SE3Quat 120 | estimateVertexSE3Expmap(SlamOptimizer &opt, int id); 121 | 122 | g2o::Vector3D 123 | estimateVertexSBAXYZ(SlamOptimizer &opt, int id); 124 | 125 | bool 126 | verifyInfo(const g2o::Matrix6d& info); 127 | 128 | bool 129 | verifyInfo(const Eigen::Matrix3d& info); 130 | 131 | 132 | }// namespace se2clam 133 | 134 | #endif // OPTIMIZER_H 135 | -------------------------------------------------------------------------------- /include/se2clam/sparsifier.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef SPARSIFIER_H 6 | #define SPARSIFIER_H 7 | 8 | #include 9 | #include 10 | 11 | struct MeasSE3XYZ { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | g2o::Vector3D z; 14 | g2o::Matrix3D info; 15 | int idMP = -1; 16 | int idKF = -1; 17 | }; 18 | 19 | struct MeasXYZ2UV { 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | g2o::Vector2D z; 22 | g2o::Matrix2D info; 23 | int idMP = -1; 24 | int idKF = -1; 25 | }; 26 | 27 | struct MeasSE3Expmap { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | g2o::SE3Quat z; 30 | g2o::Matrix6d info; 31 | int id1 = -1; 32 | int id2 = -1; 33 | }; 34 | 35 | class Sparsifier 36 | { 37 | public: 38 | Sparsifier(); 39 | 40 | static void HessianXYZ2UV(g2o::SE3Quat KF, g2o::Vector3D MP, MeasXYZ2UV measure, g2o::CameraParameters* pCamParam, 41 | Eigen::Matrix & H ); 42 | 43 | static void JacobianXYZ2UV(g2o::SE3Quat KF, g2o::Vector3D MP, g2o::CameraParameters* pCamParam, 44 | Eigen::Matrix & J); 45 | 46 | static void HessianSE3XYZ(const g2o::SE3Quat KF, const g2o::Vector3D MP, const g2o::Matrix3D info, 47 | Eigen::Matrix & H); 48 | 49 | static void JacobianSE3XYZ(const g2o::SE3Quat KF, const g2o::Vector3D MP, 50 | Eigen::Matrix & J); 51 | 52 | static void DoMarginalizeSE3XYZ(const std::vector > & vKF, const std::vector > & vMP, 53 | const std::vector > & vMeasure, 54 | g2o::SE3Quat & z_out, g2o::Matrix6d & info_out); 55 | 56 | static void JacobianSE3(const g2o::SE3Quat KF1, const g2o::SE3Quat KF2, 57 | Eigen::Matrix & J); 58 | 59 | static void InfoSE3(const g2o::SE3Quat KF1, const g2o::SE3Quat KF2, const Eigen::Matrix & info, 60 | Eigen::Matrix & H); 61 | 62 | }; 63 | 64 | #endif // SPARSIFIER_H 65 | -------------------------------------------------------------------------------- /include/se2clam/sugarCV.h: -------------------------------------------------------------------------------- 1 | // /** 2 | // This file is part of se2clam. 3 | // Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | /// 5 | // sugarCV: tool functions for opencv 6 | // By ZHEGN Fan fzheng@link.cuhk.edu.hk 7 | 8 | // Use namespace: scv (Sugar CV) 9 | 10 | #ifndef SUGAR_CV_H 11 | #define SUGAR_CV_H 12 | 13 | #include 14 | #include 15 | 16 | namespace scv{ 17 | 18 | const float BW = 5.f; 19 | const float L = 15.f; 20 | const float INRATIO = 0.996f; 21 | const float dr = 1.f; 22 | 23 | using std::vector; 24 | using cv::Mat; 25 | using cv::Point2f; 26 | using cv::Point3f; 27 | 28 | 29 | Mat inv(const Mat& T4x4); 30 | 31 | Mat sk_sym(const Point3f& _v); 32 | 33 | Point3f triangulate(const Point2f &pt1, const Point2f &pt2, const Mat &P1, const Mat &P2); 34 | 35 | Point2f camprjc(const Mat& _K, const Point3f& _pt); 36 | 37 | bool checkParallax(const Point3f& o1, const Point3f& o2, const Point3f& pt3, int minDegree = 1); 38 | 39 | Point3f se3map(const Mat& _Tcw, const Point3f& _pt); 40 | 41 | 42 | 43 | 44 | void pts2Ftrs(const vector& _orgnFtrs, const vector& _points, vector& _features); 45 | 46 | Mat drawKeysWithNum(const vector keys, const Mat &img, vector mask = vector(0, 0)); 47 | 48 | 49 | } // namespace scv 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | se2clam 4 | 0.0.0 5 | The se2clam package 6 | 7 | 8 | 9 | 10 | fzheng 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | message_generation 37 | 38 | 39 | 40 | 41 | 42 | message_runtime 43 | 44 | 45 | 46 | 47 | catkin 48 | cmake_modules 49 | geometry_msgs 50 | nav_msgs 51 | roscpp 52 | rospy 53 | tf 54 | image_transport 55 | 56 | 57 | geometry_msgs 58 | nav_msgs 59 | roscpp 60 | rospy 61 | tf 62 | cv_bridge 63 | image_transport 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 101 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Marker1 10 | - /Marker1/Namespaces1 11 | - /Image1 12 | Splitter Ratio: 0.5 13 | Tree Height: 411 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Image 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Class: rviz/Marker 37 | Enabled: true 38 | Marker Topic: /se2clam/Map 39 | Name: Marker 40 | Namespaces: 41 | Camera: true 42 | CovisGraph: true 43 | FeatGraph: true 44 | KeyFramesActive: true 45 | KeyFramesNegative: true 46 | MapPointsActive: true 47 | MapPointsNegative: true 48 | MapPointsNow: true 49 | OdoGraph: true 50 | Queue Size: 100 51 | Value: true 52 | - Class: rviz/Image 53 | Enabled: true 54 | Image Topic: /camera/framepub 55 | Max Value: 1 56 | Median window: 5 57 | Min Value: 0 58 | Name: Image 59 | Normalize Range: true 60 | Queue Size: 2 61 | Transport Hint: raw 62 | Unreliable: false 63 | Value: true 64 | Enabled: true 65 | Global Options: 66 | Background Color: 255; 255; 255 67 | Default Light: true 68 | Fixed Frame: se2clam/World 69 | Frame Rate: 10 70 | Name: root 71 | Tools: 72 | - Class: rviz/Interact 73 | Hide Inactive Objects: true 74 | - Class: rviz/MoveCamera 75 | - Class: rviz/Select 76 | - Class: rviz/FocusCamera 77 | - Class: rviz/Measure 78 | - Class: rviz/SetInitialPose 79 | Topic: /initialpose 80 | - Class: rviz/SetGoal 81 | Topic: /move_base_simple/goal 82 | - Class: rviz/PublishPoint 83 | Single click: true 84 | Topic: /clicked_point 85 | Value: true 86 | Views: 87 | Current: 88 | Class: rviz/Orbit 89 | Distance: 87.7072983 90 | Enable Stereo Rendering: 91 | Stereo Eye Separation: 0.0599999987 92 | Stereo Focal Distance: 1 93 | Swap Stereo Eyes: false 94 | Value: false 95 | Focal Point: 96 | X: -0.262300998 97 | Y: -9.61233044 98 | Z: 3.18214011 99 | Focal Shape Fixed Size: true 100 | Focal Shape Size: 0.0500000007 101 | Invert Z Axis: false 102 | Name: Current View 103 | Near Clip Distance: 0.00999999978 104 | Pitch: 1.4598 105 | Target Frame: se2clam/World 106 | Value: Orbit (rviz) 107 | Yaw: 1.40540004 108 | Saved: ~ 109 | Window Geometry: 110 | Displays: 111 | collapsed: false 112 | Height: 1027 113 | Hide Left Dock: false 114 | Hide Right Dock: true 115 | Image: 116 | collapsed: false 117 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000241000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026f000001320000001600ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000379000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000045f0000003efc0100000002fb0000000800540069006d006501000000000000045f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002ef0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 118 | Selection: 119 | collapsed: false 120 | Time: 121 | collapsed: false 122 | Tool Properties: 123 | collapsed: false 124 | Views: 125 | collapsed: true 126 | Width: 1119 127 | X: 801 128 | Y: 48 129 | -------------------------------------------------------------------------------- /rviz.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=1 2 | Background\ ColorG=1 3 | Background\ ColorR=1 4 | Camera\ Config=-1.3748 4.59369 4.31087 0.0334986 -0.381037 1.14662 5 | Camera\ Type=rviz::OrbitViewController 6 | Fixed\ Frame=/se2clam/Camera 7 | Marker.Camera=1 8 | Marker.Enabled=1 9 | Marker.Graph=1 10 | Marker.KeyFrames=1 11 | Marker.MapPoints=1 12 | Marker.Marker\ Topic=/se2clam/Map 13 | Marker.Queue\ Size=100 14 | Property\ Grid\ Splitter=502,78 15 | Property\ Grid\ State=expanded=.Global Options,Marker.Enabled;splitterratio=0.5 16 | QMainWindow=000000ff00000000fd00000003000000000000011d00000296fc0200000002fb000000100044006900730070006c006100790073010000001d00000296000000ee00fffffffb0000000a00560069006500770073000000011c000000f8000000bb00ffffff00000001000001310000026ffc0200000002fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000000a00000006700fffffffb0000001200530065006c0065006300740069006f006e00000001b1000000db0000006700ffffff00000003000003910000003efc0100000001fb0000000800540069006d0065010000000000000391000002bf00ffffff0000026e0000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 17 | Target\ Frame=/se2clam/Camera 18 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 19 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 20 | [Display0] 21 | ClassName=rviz::MarkerDisplay 22 | Name=Marker 23 | [Window] 24 | Height=795 25 | Width=929 26 | X=734 27 | Y=-28 28 | -------------------------------------------------------------------------------- /src/Config.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "Config.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace se2clam{ 11 | 12 | #ifndef M_PI 13 | #define M_PI 3.14159265358979323846 14 | #endif 15 | 16 | using namespace std; 17 | 18 | std::string Config::DataPath; 19 | int Config::ImgIndex; 20 | int Config::ImgIndexLocalSt; 21 | cv::Size Config::ImgSize; 22 | cv::Mat Config::bTc; // camera extrinsic 23 | cv::Mat Config::cTb; // inv of bTc 24 | cv::Mat Config::Kcam; // camera intrinsic 25 | float Config::fxCam; 26 | float Config::fyCam; 27 | cv::Mat Config::Dcam; // camera distortion 28 | 29 | float Config::UPPER_DEPTH; 30 | float Config::LOWER_DEPTH; 31 | 32 | int Config::NUM_FILTER_LAST_SEVERAL_MU; 33 | int Config::FILTER_CONVERGE_CONTINUE_COUNT; 34 | float Config::DEPTH_FILTER_THRESHOLD; 35 | 36 | float Config::ScaleFactor; // scalefactor in detecting features 37 | int Config::MaxLevel; // level number of pyramid in detecting features 38 | int Config::MaxFtrNumber; // max feature number to detect 39 | float Config::FEATURE_SIGMA; 40 | 41 | float Config::ODO_X_UNCERTAIN, Config::ODO_Y_UNCERTAIN, Config::ODO_T_UNCERTAIN; 42 | float Config::ODO_X_NOISE, Config::ODO_Y_NOISE, Config::ODO_T_NOISE; 43 | 44 | float Config::PLANEMOTION_XROT_INFO = 1e6; 45 | float Config::PLANEMOTION_YROT_INFO = 1e6; 46 | float Config::PLANEMOTION_Z_INFO = 1; 47 | 48 | int Config::LOCAL_FRAMES_NUM; 49 | float Config::TH_HUBER; 50 | int Config::LOCAL_ITER; 51 | bool Config::LOCAL_VERBOSE = false; 52 | int Config::GLOBAL_ITER = 15; 53 | bool Config::GLOBAL_VERBOSE = false; 54 | bool Config::LOCAL_PRINT = false; 55 | bool Config::GLOBAL_PRINT = false; 56 | 57 | int Config::FPS; 58 | 59 | bool Config::USE_PREV_MAP = false; 60 | bool Config::LOCALIZATION_ONLY = false; 61 | bool Config::SAVE_NEW_MAP = false; 62 | std::string Config::READ_MAP_FILE_NAME; 63 | std::string Config::READ_MAP_FILE_PATH; 64 | std::string Config::WRITE_MAP_FILE_NAME = "se2clam.map"; 65 | std::string Config::WRITE_MAP_FILE_PATH = "/home/se2clam/"; 66 | 67 | std::string Config::WRITE_TRAJ_FILE_NAME; 68 | std::string Config::WRITE_TRAJ_FILE_PATH; 69 | 70 | cv::Mat Config::PrjMtrxEye; 71 | 72 | int Config::MAPPUB_SCALE_RATIO = 300; 73 | 74 | int Config::GM_VCL_NUM_MIN_MATCH_MP = 15; 75 | int Config::GM_VCL_NUM_MIN_MATCH_KP = 30; 76 | double Config::GM_VCL_RATIO_MIN_MATCH_MP = 0.05; 77 | 78 | int Config::GM_DCL_MIN_KFID_OFFSET = 20; 79 | double Config::GM_DCL_MIN_SCORE_BEST = 0.005; 80 | 81 | void Config::readConfig(const std::string &path){ 82 | DataPath = path; 83 | std::string camParaPath = path + "/config/CamConfig.yml"; 84 | cv::FileStorage camPara(camParaPath, cv::FileStorage::READ); 85 | assert(camPara.isOpened()); 86 | cv::Mat _mK, _mD, _rvec, rvec, _T, T, R; 87 | float height, width; 88 | camPara["image_height"] >> height; 89 | camPara["image_width"] >> width; 90 | camPara["camera_matrix"] >> _mK; 91 | camPara["distortion_coefficients"] >> _mD; 92 | camPara["rvec_b_c"] >> _rvec; 93 | camPara["tvec_b_c"] >> _T; 94 | _mK.convertTo(Kcam,CV_32FC1); 95 | _mD.convertTo(Dcam,CV_32FC2); 96 | _rvec.convertTo(rvec,CV_32FC1); 97 | _T.convertTo(T,CV_32FC1); 98 | fxCam = Kcam.at(0,0); 99 | fyCam = Kcam.at(1,1); 100 | ImgSize.height = height; 101 | ImgSize.width = width; 102 | std::cerr << "# Load camera config ..." << std::endl; 103 | std::cerr << "- Camera matrix: " << std::endl << " " << 104 | Kcam << std::endl << 105 | "- Camera distortion: " << std::endl << " " << 106 | Dcam << std::endl << 107 | "- Img size: " << std::endl << " " << 108 | ImgSize << std::endl << std::endl; 109 | // bTc: camera extrinsic 110 | cv::Rodrigues(rvec,R); 111 | bTc = cv::Mat::eye(4,4,CV_32FC1); 112 | R.copyTo(bTc.rowRange(0,3).colRange(0,3)); 113 | T.copyTo(bTc.rowRange(0,3).col(3)); 114 | cv::Mat RT = R.t(); 115 | cv::Mat t = -RT * T; 116 | cTb = cv::Mat::eye(4,4,CV_32FC1); 117 | RT.copyTo(cTb.rowRange(0,3).colRange(0,3)); 118 | t.copyTo(cTb.rowRange(0,3).col(3)); 119 | 120 | 121 | PrjMtrxEye = Kcam * cv::Mat::eye(3,4,CV_32FC1); 122 | camPara.release(); 123 | 124 | std::string settingsPath = path + "/config/Settings.yml"; 125 | cv::FileStorage settings(settingsPath, cv::FileStorage::READ); 126 | assert(settings.isOpened()); 127 | 128 | ImgIndex = (int)settings["img_num"]; 129 | ImgIndexLocalSt = (int)settings["img_id_local_st"]; 130 | UPPER_DEPTH = (float)settings["upper_depth"]; 131 | LOWER_DEPTH = (float)settings["lower_depth"]; 132 | NUM_FILTER_LAST_SEVERAL_MU = (int)settings["depth_filter_avrg_count"]; 133 | FILTER_CONVERGE_CONTINUE_COUNT = (int)settings["depth_filter_converge_count"]; 134 | DEPTH_FILTER_THRESHOLD = (float)settings["depth_filter_thresh"]; 135 | ScaleFactor = (float)settings["scale_facotr"]; 136 | MaxLevel = (int)settings["max_level"]; 137 | MaxFtrNumber = (int)settings["max_feature_num"]; 138 | FEATURE_SIGMA = (float)settings["feature_sigma"]; 139 | 140 | ODO_X_UNCERTAIN = (float)settings["odo_x_uncertain"]; 141 | ODO_Y_UNCERTAIN = (float)settings["odo_y_uncertain"]; 142 | ODO_T_UNCERTAIN = (float)settings["odo_theta_uncertain"]; 143 | ODO_X_NOISE = (float)settings["odo_x_steady_noise"]; 144 | ODO_Y_NOISE = (float)settings["odo_y_steady_noise"]; 145 | ODO_T_NOISE = (float)settings["odo_theta_steady_noise"]; 146 | if(!settings["plane_motion_xrot_info"].empty()) 147 | PLANEMOTION_XROT_INFO = (float)settings["plane_motion_xrot_info"]; 148 | if(!settings["plane_motion_yrot_info"].empty()) 149 | PLANEMOTION_YROT_INFO = (float)settings["plane_motion_yrot_info"]; 150 | if(!settings["plane_motion_z_info"].empty()) 151 | PLANEMOTION_Z_INFO = (float)settings["plane_motion_z_info"]; 152 | LOCAL_FRAMES_NUM = (int)settings["frame_num"]; 153 | TH_HUBER = sqrt((float)settings["th_huber2"]); 154 | LOCAL_ITER = (int)settings["local_iter"]; 155 | LOCAL_VERBOSE = (bool)(int)(settings["local_verbose"]); 156 | LOCAL_PRINT = (bool)(int)(settings["local_print"]); 157 | if((int)settings["global_iter"]){ 158 | GLOBAL_ITER = (int)settings["global_iter"]; 159 | } 160 | GLOBAL_VERBOSE = (bool)(int)(settings["global_verbose"]); 161 | GLOBAL_PRINT = (bool)(int)(settings["global_print"]); 162 | FPS = (int)settings["fps"]; 163 | 164 | USE_PREV_MAP = (bool)(int)(settings["use_prev_map"]); 165 | SAVE_NEW_MAP = (bool)(int)(settings["save_new_map"]); 166 | LOCALIZATION_ONLY = (bool)(int)(settings["localization_only"]); 167 | settings["read_map_file_name"] >> READ_MAP_FILE_NAME; 168 | settings["write_map_file_name"] >> WRITE_MAP_FILE_NAME; 169 | settings["read_map_file_path"] >> READ_MAP_FILE_PATH; 170 | settings["write_map_file_path"] >> WRITE_MAP_FILE_PATH; 171 | settings["write_traj_file_name"] >> WRITE_TRAJ_FILE_NAME; 172 | settings["write_traj_file_path"] >> WRITE_TRAJ_FILE_PATH; 173 | 174 | MAPPUB_SCALE_RATIO = (int)(settings["mappub_scale_ratio"]); 175 | 176 | GM_VCL_NUM_MIN_MATCH_MP = (int)(settings["gm_vcl_num_min_match_mp"]); 177 | GM_VCL_NUM_MIN_MATCH_KP = (int)(settings["gm_vcl_num_min_match_kp"]); 178 | GM_VCL_RATIO_MIN_MATCH_MP = (double)(settings["gm_vcl_ratio_min_match_kp"]); 179 | 180 | GM_DCL_MIN_KFID_OFFSET = (int)(settings["gm_dcl_min_kfid_offset"]); 181 | GM_DCL_MIN_SCORE_BEST = (double)(settings["gm_dcl_min_score_best"]); 182 | 183 | settings.release(); 184 | } 185 | 186 | bool Config::acceptDepth(float depth){ 187 | return (depth >= LOWER_DEPTH && depth <= UPPER_DEPTH); 188 | } 189 | 190 | 191 | Se2::Se2(){} 192 | Se2::Se2(float _x, float _y ,float _theta): 193 | x(_x), y(_y), theta(_theta){} 194 | Se2::~Se2(){} 195 | 196 | Se2 Se2::operator +(const Se2& toadd){ 197 | // Note: dx and dy, which is expressed in the previous, 198 | // should be transformed to be expressed in the world frame 199 | float cost = std::cos(theta); 200 | float sint = std::sin(theta); 201 | float _x = x + toadd.x*cost - toadd.y*sint; 202 | float _y = y + toadd.x*sint + toadd.y*cost; 203 | float _theta = theta + toadd.theta; 204 | return Se2(_x, _y, _theta); 205 | } 206 | 207 | Se2 Se2::operator -(const Se2& tominus){ 208 | double PI = M_PI; 209 | 210 | float dx = x - tominus.x; 211 | float dy = y - tominus.y; 212 | float dtheta = theta - tominus.theta; 213 | 214 | dtheta = dtheta - floor(dtheta/(2*PI))*2*PI; 215 | 216 | if (dtheta > PI) { 217 | dtheta -= 2*PI; 218 | } 219 | 220 | float cost = std::cos(tominus.theta); 221 | float sint = std::sin(tominus.theta); 222 | // Note: dx and dy, which is expressed in world frame, 223 | // should be transformed to be expressed in the previous frame 224 | return Se2(cost*dx+sint*dy, -sint*dx+cost*dy, dtheta); 225 | } 226 | 227 | cv::Mat Se2::toCvSE3() 228 | { 229 | float c = cos(theta); 230 | float s = sin(theta); 231 | 232 | return (cv::Mat_(4,4) << 233 | c,-s, 0, x, 234 | s, c, 0, y, 235 | 0, 0, 1, 0, 236 | 0, 0, 0, 1); 237 | } 238 | 239 | 240 | } 241 | -------------------------------------------------------------------------------- /src/Frame.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "Frame.h" 6 | #include "KeyFrame.h" 7 | #include "converter.h" 8 | #include "Track.h" 9 | #include "sugarCV.h" 10 | 11 | namespace se2clam { 12 | using namespace cv; 13 | using namespace std; 14 | 15 | Frame::Frame(){} 16 | 17 | Frame::Frame(const Mat &im, const Se2& odo, ORBextractor *extractor, const Mat &K, const Mat &distCoef){ 18 | 19 | mpORBExtractor = extractor; 20 | undistort(im, img, Config::Kcam, Config::Dcam); 21 | //im.copyTo(img); 22 | 23 | (*mpORBExtractor)(img, cv::Mat(), keyPoints, descriptors); 24 | 25 | N = keyPoints.size(); 26 | if(keyPoints.empty()) 27 | return; 28 | 29 | //undistortKeyPoints(K, distCoef); 30 | keyPointsUn = keyPoints; 31 | 32 | if(mbInitialComputations){ 33 | computeBoundUn(K, distCoef); 34 | 35 | mfGridElementWidthInv = static_cast(FRAME_GRID_COLS)/(maxXUn-minXUn); 36 | mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS)/(maxYUn-minYUn); 37 | 38 | mbInitialComputations = false; 39 | } 40 | 41 | id = nextId; 42 | nextId++; 43 | 44 | //Scale Levels Info 45 | mnScaleLevels = mpORBExtractor->GetLevels(); 46 | mfScaleFactor = mpORBExtractor->GetScaleFactor(); 47 | 48 | mvScaleFactors.resize(mnScaleLevels); 49 | mvLevelSigma2.resize(mnScaleLevels); 50 | mvScaleFactors[0] = 1.0f; 51 | mvLevelSigma2[0] = 1.0f; 52 | for(int i=1; i(0) == 0.){ 164 | return; 165 | } 166 | Mat_ mat(1,keyPoints.size()); 167 | for(size_t i=0; i(0) == 0.){ 181 | minXUn = 0.f; 182 | minYUn = 0.f; 183 | maxXUn = x; 184 | maxYUn = y; 185 | return; 186 | } 187 | Mat_ mat(1,4); 188 | mat << Point2f(0,0), Point2f(x,0), Point2f(0,y), Point2f(x,y); 189 | undistortPoints(mat,mat,K,D,Mat(),K); 190 | minXUn = std::min(mat(0).x,mat(2).x); 191 | minYUn = std::min(mat(0).y,mat(1).y); 192 | maxXUn = std::max(mat(1).x,mat(3).x); 193 | maxYUn = std::max(mat(2).y,mat(3).y); 194 | } 195 | 196 | bool Frame::inImgBound(Point2f pt){ 197 | return (pt.x >= minXUn && pt.x <= maxXUn && 198 | pt.y >= minYUn && pt.y <=maxYUn); 199 | } 200 | 201 | 202 | // From ORB_SLAM 203 | bool Frame::PosInGrid(cv::KeyPoint &kp, int &posX, int &posY) 204 | { 205 | posX = round((kp.pt.x-minXUn)*mfGridElementWidthInv); 206 | posY = round((kp.pt.y-minYUn)*mfGridElementHeightInv); 207 | 208 | //Keypoint's coordinates are undistorted, which could cause to go out of the image 209 | if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS) 210 | return false; 211 | 212 | return true; 213 | } 214 | 215 | // From ORB_SLAM 216 | vector Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, int minLevel, int maxLevel) const 217 | { 218 | vector vIndices; 219 | vIndices.reserve(keyPointsUn.size()); 220 | 221 | int nMinCellX = floor((x-minXUn-r)*mfGridElementWidthInv); 222 | nMinCellX = max(0,nMinCellX); 223 | if(nMinCellX>=FRAME_GRID_COLS) 224 | return vIndices; 225 | 226 | int nMaxCellX = ceil((x-minXUn+r)*mfGridElementWidthInv); 227 | nMaxCellX = min(FRAME_GRID_COLS-1,nMaxCellX); 228 | if(nMaxCellX<0) 229 | return vIndices; 230 | 231 | int nMinCellY = floor((y-minYUn-r)*mfGridElementHeightInv); 232 | nMinCellY = max(0,nMinCellY); 233 | if(nMinCellY>=FRAME_GRID_ROWS) 234 | return vIndices; 235 | 236 | int nMaxCellY = ceil((y-minYUn+r)*mfGridElementHeightInv); 237 | nMaxCellY = min(FRAME_GRID_ROWS-1,nMaxCellY); 238 | if(nMaxCellY<0) 239 | return vIndices; 240 | 241 | bool bCheckLevels=true; 242 | bool bSameLevel=false; 243 | if(minLevel==-1 && maxLevel==-1) 244 | bCheckLevels=false; 245 | else 246 | if(minLevel==maxLevel) 247 | bSameLevel=true; 248 | 249 | for(int ix = nMinCellX; ix<=nMaxCellX; ix++) 250 | { 251 | for(int iy = nMinCellY; iy<=nMaxCellY; iy++) 252 | { 253 | vector vCell = mGrid[ix][iy]; 254 | if(vCell.empty()) 255 | continue; 256 | 257 | for(size_t j=0, jend=vCell.size(); jmaxLevel) 263 | continue; 264 | } 265 | else if(bSameLevel) 266 | { 267 | if(kpUn.octave!=minLevel) 268 | continue; 269 | } 270 | 271 | if(abs(kpUn.pt.x-x)>r || abs(kpUn.pt.y-y)>r) 272 | continue; 273 | 274 | vIndices.push_back(vCell[j]); 275 | } 276 | } 277 | } 278 | 279 | return vIndices; 280 | } 281 | 282 | void Frame::copyImgTo(cv::Mat & imgRet) { 283 | lock_guard lock(mMutexImg); 284 | img.copyTo(imgRet); 285 | } 286 | 287 | void Frame::copyDesTo(cv::Mat & desRet) { 288 | lock_guard lock(mMutexDes); 289 | descriptors.copyTo(desRet); 290 | } 291 | 292 | Frame::~Frame(){} 293 | 294 | } 295 | 296 | 297 | 298 | 299 | -------------------------------------------------------------------------------- /src/FramePublish.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "FramePublish.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "sugarCV.h" 11 | #include "Track.h" 12 | #include "GlobalMapper.h" 13 | #include "Localizer.h" 14 | 15 | namespace se2clam{ 16 | 17 | using namespace cv; 18 | using std::vector; 19 | 20 | typedef lock_guard locker; 21 | 22 | FramePublish::FramePublish(){ 23 | 24 | } 25 | 26 | FramePublish::FramePublish(Track* pTR, GlobalMapper* pGM){ 27 | mpTrack = pTR; 28 | mpGM = pGM; 29 | mbIsLocalize = false; 30 | } 31 | 32 | FramePublish::~FramePublish(){ 33 | 34 | } 35 | 36 | void FramePublish::run(){ 37 | 38 | ros::NodeHandle nh; 39 | image_transport::ImageTransport it(nh); 40 | image_transport::Publisher pub = it.advertise("/camera/framepub",1); 41 | 42 | float fps = Config::FPS; 43 | ros::Rate rate(fps); 44 | 45 | while(nh.ok() && ros::ok()){ 46 | 47 | if (!mbIsLocalize) { 48 | if( mpTrack->copyForPub(kpRef, kp, mImgRef, mImg, matches) ){ 49 | 50 | WorkTimer timer; 51 | timer.start(); 52 | 53 | Mat imgCurr = drawMatchesInOneImg(kpRef, mImg, kp, matches); 54 | Mat imgRef = drawKeys(kpRef, mImgRef, matches); 55 | Mat imgMatch; 56 | mpGM->mImgMatch.copyTo(imgMatch); 57 | Size sizeImgCurr = imgCurr.size(); 58 | Size sizeImgMatch = imgMatch.size(); 59 | 60 | Mat imgOut(sizeImgCurr.height*2, sizeImgCurr.width*2, imgCurr.type(), Scalar(0)); 61 | 62 | imgCurr.copyTo(imgOut(cv::Rect(0,0,sizeImgCurr.width,sizeImgCurr.height))); 63 | imgRef.copyTo(imgOut(cv::Rect(0,sizeImgCurr.height,sizeImgCurr.width,sizeImgCurr.height))); 64 | if (sizeImgMatch.width != 0) { 65 | imgMatch.copyTo(imgOut(cv::Rect(sizeImgCurr.width,0,sizeImgMatch.width,sizeImgMatch.height))); 66 | } 67 | 68 | timer.stop(); 69 | cv::resize(imgOut, imgOut, cv::Size(640,480)); 70 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgOut).toImageMsg(); 71 | pub.publish(msg); 72 | 73 | } 74 | } 75 | else { 76 | 77 | locker lockImg(mpLocalizer->mMutexImg); 78 | 79 | if (mpLocalizer == NULL) continue; 80 | if (mpLocalizer->mpKFCurr == NULL) continue; 81 | if (mpLocalizer->mImgCurr.cols == 0) continue; 82 | 83 | Mat imgCurr; 84 | mpLocalizer->mImgCurr.copyTo(imgCurr); 85 | Size sizeImgCurr = imgCurr.size(); 86 | 87 | Mat imgOut(sizeImgCurr.height*2, sizeImgCurr.width*2, imgCurr.type(), Scalar(0)); 88 | imgCurr.copyTo(imgOut(cv::Rect(0, 0, sizeImgCurr.width, sizeImgCurr.height))); 89 | 90 | if (mpLocalizer->mImgLoop.cols != 0) { 91 | Mat imgLoop; 92 | mpLocalizer->mImgLoop.copyTo(imgLoop); 93 | imgLoop.copyTo(imgOut(cv::Rect(0, sizeImgCurr.height, sizeImgCurr.width, sizeImgCurr.height))); 94 | } 95 | 96 | Mat imgMatch; 97 | mpLocalizer->mImgMatch.copyTo(imgMatch); 98 | Size sizeImgMatch = imgMatch.size(); 99 | if (sizeImgMatch.width != 0) { 100 | imgMatch.copyTo(imgOut(cv::Rect(sizeImgCurr.width,0,sizeImgMatch.width,sizeImgMatch.height))); 101 | } 102 | 103 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgOut).toImageMsg(); 104 | pub.publish(msg); 105 | } 106 | 107 | rate.sleep(); 108 | } 109 | } 110 | 111 | cv::Mat FramePublish::drawMatchesInOneImg(const vector queryKeys, const Mat &trainImg, 112 | const vector trainKeys, const vector &matches){ 113 | Mat out = trainImg.clone(); 114 | if (trainImg.channels() == 1) 115 | cvtColor(trainImg, out, CV_GRAY2BGR); 116 | for (unsigned i = 0; i < matches.size(); i++) { 117 | 118 | if (matches[i] < 0) { 119 | continue; 120 | // Point2f ptRef = queryKeys[i].pt; 121 | // circle(out, ptRef, 5, Scalar(255, 0, 0), 1); 122 | } 123 | else { 124 | Point2f ptRef = queryKeys[i].pt; 125 | Point2f ptCurr = trainKeys[matches[i]].pt; 126 | circle(out, ptCurr, 5, Scalar(0, 255, 0), 1); 127 | circle(out, ptRef, 5, Scalar(0, 0, 255), 1); 128 | line(out, ptRef, ptCurr, Scalar(0, 255, 0)); 129 | } 130 | } 131 | return out.clone(); 132 | } 133 | 134 | cv::Mat FramePublish::drawKeys(const vector keys, const Mat &img, vector matched){ 135 | Mat out = img.clone(); 136 | if (img.channels() == 1) 137 | cvtColor(img, out, CV_GRAY2BGR); 138 | for (unsigned i = 0; i < matched.size(); i++) { 139 | Point2f pt1 = keys[i].pt; 140 | if (matched[i] < 0) { 141 | circle(out, pt1, 5, Scalar(255, 0, 0), 1); 142 | } 143 | else { 144 | circle(out, pt1, 5, Scalar(0, 0, 255), 1); 145 | } 146 | } 147 | return out.clone(); 148 | } 149 | 150 | Mat FramePublish::drawFrame() { 151 | 152 | if (!mbIsLocalize) { 153 | if (mpTrack->copyForPub(kpRef, kp, mImgRef, mImg, matches)){ 154 | 155 | Mat imgCurr = drawMatchesInOneImg(kpRef, mImg, kp, matches); 156 | Mat imgRef = drawKeys(kpRef, mImgRef, matches); 157 | Mat imgMatch; 158 | mpGM->mImgMatch.copyTo(imgMatch); 159 | Size sizeImgCurr = imgCurr.size(); 160 | Size sizeImgMatch = imgMatch.size(); 161 | 162 | Mat imgOut(sizeImgCurr.height * 2, sizeImgCurr.width * 2, imgCurr.type(), Scalar(0)); 163 | 164 | imgCurr.copyTo(imgOut(cv::Rect(0, 0, sizeImgCurr.width, sizeImgCurr.height))); 165 | imgRef.copyTo(imgOut(cv::Rect(0, sizeImgCurr.height, sizeImgCurr.width, sizeImgCurr.height))); 166 | if (sizeImgMatch.width != 0) { 167 | imgMatch.copyTo(imgOut(cv::Rect(sizeImgCurr.width, 0, sizeImgMatch.width, sizeImgMatch.height))); 168 | } 169 | 170 | imgOut.copyTo(mImgOut); 171 | } 172 | } 173 | else if (mpLocalizer != NULL && mpLocalizer->mpKFCurr != NULL && mpLocalizer->mImgCurr.cols != 0){ 174 | 175 | locker lockImg(mpLocalizer->mMutexImg); 176 | 177 | Mat imgCurr; 178 | mpLocalizer->mImgCurr.copyTo(imgCurr); 179 | Size sizeImgCurr = imgCurr.size(); 180 | 181 | Mat imgOut(sizeImgCurr.height * 2, sizeImgCurr.width * 2, imgCurr.type(), Scalar(0)); 182 | imgCurr.copyTo(imgOut(cv::Rect(0, 0, sizeImgCurr.width, sizeImgCurr.height))); 183 | 184 | if (mpLocalizer->mImgLoop.cols != 0) { 185 | Mat imgLoop; 186 | mpLocalizer->mImgLoop.copyTo(imgLoop); 187 | imgLoop.copyTo(imgOut(cv::Rect(0, sizeImgCurr.height, sizeImgCurr.width, sizeImgCurr.height))); 188 | } 189 | 190 | Mat imgMatch; 191 | mpLocalizer->mImgMatch.copyTo(imgMatch); 192 | Size sizeImgMatch = imgMatch.size(); 193 | if (sizeImgMatch.width != 0) { 194 | imgMatch.copyTo(imgOut(cv::Rect(sizeImgCurr.width, 0, sizeImgMatch.width, sizeImgMatch.height))); 195 | } 196 | 197 | imgOut.copyTo(mImgOut); 198 | } 199 | 200 | return mImgOut.clone(); 201 | } 202 | 203 | 204 | void FramePublish::setLocalizer(Localizer* localizer){ 205 | mpLocalizer = localizer; 206 | } 207 | 208 | 209 | }// namespace se2clam 210 | -------------------------------------------------------------------------------- /src/OdoSLAM.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #ifndef ODOSLAM_CPP 6 | #define ODOSLAM_CPP 7 | #include "OdoSLAM.h" 8 | #include 9 | 10 | #endif // ODOSLAM_CPP 11 | 12 | namespace se2clam { 13 | using namespace std; 14 | using namespace cv; 15 | 16 | std::mutex mMutexFinish; 17 | 18 | OdoSLAM::~OdoSLAM(){ 19 | delete mpMapPub; 20 | delete mpLocalizer; 21 | delete mpTrack; 22 | delete mpLocalMapper; 23 | delete mpGlobalMapper; 24 | delete mpMap; 25 | delete mpMapStorage; 26 | delete mpFramePub; 27 | delete mpSensors; 28 | } 29 | 30 | OdoSLAM::OdoSLAM(){ 31 | 32 | } 33 | 34 | void OdoSLAM::setVocFileBin(const char *strVoc){ 35 | cerr << "\n###\n" 36 | << "### se2clam: SE(2)-Constrained Visual Odometric Localization and Mapping\n" 37 | << "###\n" << endl; 38 | 39 | //Init ORB BoW 40 | cerr << endl << "Loading ORB Vocabulary. This could take a while." << endl; 41 | string strVocFile = strVoc; 42 | mpVocabulary = new ORBVocabulary(); 43 | bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile); 44 | if (!bVocLoad) { 45 | cerr << "Wrong path to vocabulary. " << endl; 46 | cerr << "Falied to open at: " << strVocFile << endl; 47 | return; 48 | } 49 | cerr << "Vocabulary loaded!" << endl << endl; 50 | } 51 | 52 | void OdoSLAM::setDataPath(const char *strDataPath){ 53 | 54 | Config::readConfig(strDataPath); 55 | 56 | } 57 | 58 | cv::Mat OdoSLAM::getCurrentVehiclePose() 59 | { 60 | return scv::inv( mpMap->getCurrentFramePose() ) * Config::cTb; 61 | } 62 | 63 | cv::Mat OdoSLAM::getCurrentCameraPoseWC() 64 | { 65 | return scv::inv( mpMap->getCurrentFramePose() ); 66 | } 67 | 68 | cv::Mat OdoSLAM::getCurrentCameraPoseCW() 69 | { 70 | return mpMap->getCurrentFramePose(); 71 | } 72 | 73 | void OdoSLAM::start() { 74 | 75 | // Construct the system 76 | mpMap = new Map; 77 | mpSensors = new Sensors; 78 | mpTrack = new Track; 79 | mpLocalMapper = new LocalMapper; 80 | mpGlobalMapper = new GlobalMapper; 81 | mpFramePub = new FramePublish(mpTrack, mpGlobalMapper); 82 | mpMapStorage = new MapStorage(); 83 | mpMapPub = new MapPublish(mpMap); 84 | mpLocalizer = new Localizer(); 85 | 86 | mpTrack->setLocalMapper(mpLocalMapper); 87 | mpTrack->setMap(mpMap); 88 | mpTrack->setSensors(mpSensors); 89 | 90 | mpLocalMapper->setMap(mpMap); 91 | mpLocalMapper->setGlobalMapper(mpGlobalMapper); 92 | 93 | mpGlobalMapper->setMap(mpMap); 94 | mpGlobalMapper->setLocalMapper(mpLocalMapper); 95 | mpGlobalMapper->setORBVoc(mpVocabulary); 96 | 97 | mpMapStorage->setMap(mpMap); 98 | 99 | mpMapPub->setFramePub(mpFramePub); 100 | 101 | mpLocalizer->setMap(mpMap); 102 | mpLocalizer->setORBVoc(mpVocabulary); 103 | mpLocalizer->setSensors(mpSensors); 104 | 105 | 106 | mpFramePub->setLocalizer(mpLocalizer); 107 | mpMapPub->setLocalizer(mpLocalizer); 108 | 109 | 110 | if (Config::USE_PREV_MAP){ 111 | mpMapStorage->setFilePath(Config::READ_MAP_FILE_PATH, Config::READ_MAP_FILE_NAME); 112 | mpMapStorage->loadMap(); 113 | } 114 | 115 | mbFinishRequested = false; 116 | mbFinished = false; 117 | 118 | if (se2clam::Config::LOCALIZATION_ONLY) { 119 | 120 | thread threadLocalizer(&se2clam::Localizer::run, mpLocalizer); 121 | 122 | mpFramePub->mbIsLocalize = true; 123 | mpMapPub->mbIsLocalize = true; 124 | 125 | thread threadMapPub(&se2clam::MapPublish::run, mpMapPub); 126 | 127 | threadLocalizer.detach(); 128 | threadMapPub.detach(); 129 | 130 | } 131 | // SLAM case 132 | else { 133 | 134 | cout << "Running SLAM" << endl; 135 | 136 | mpMapPub->mbIsLocalize = false; 137 | mpFramePub->mbIsLocalize = false; 138 | 139 | 140 | thread threadTracker(&se2clam::Track::run, mpTrack); 141 | thread threadLocalMapper(&se2clam::LocalMapper::run, mpLocalMapper); 142 | thread threadGlobalMapper(&se2clam::GlobalMapper::run, mpGlobalMapper); 143 | thread threadMapPub(&se2clam::MapPublish::run, mpMapPub); 144 | 145 | threadTracker.detach(); 146 | threadLocalMapper.detach(); 147 | threadGlobalMapper.detach(); 148 | threadMapPub.detach(); 149 | 150 | } 151 | 152 | thread threadWait(&wait, this); 153 | threadWait.detach(); 154 | 155 | } 156 | 157 | void OdoSLAM::wait(OdoSLAM* system){ 158 | 159 | ros::Rate rate(Config::FPS * 10); 160 | cv::Mat empty(100, 640, CV_8U, cv::Scalar(0)); 161 | 162 | //cv::namedWindow("Press q on this window to exit..."); 163 | while (1) { 164 | if (system->checkFinish()) { 165 | 166 | system->sendRequestFinish(); 167 | 168 | break; 169 | } 170 | //cv::imshow("Press q on this window to exit...", empty); 171 | if(cv::waitKey(5) == 'q'){ 172 | system->requestFinish(); 173 | } 174 | rate.sleep(); 175 | } 176 | //cv::destroyAllWindows(); 177 | 178 | system->saveMap(); 179 | 180 | system->checkAllExit(); 181 | 182 | system->clear(); 183 | 184 | system->mbFinished = true; 185 | 186 | cerr << "System is cleared .." << endl; 187 | 188 | } 189 | 190 | void OdoSLAM::saveMap() { 191 | if (se2clam::Config::SAVE_NEW_MAP){ 192 | mpMapStorage->setFilePath(se2clam::Config::WRITE_MAP_FILE_PATH, se2clam::Config::WRITE_MAP_FILE_NAME); 193 | printf("&& DBG MS: Begin save map.\n"); 194 | mpMapStorage->saveMap(); 195 | } 196 | 197 | // Save keyframe trajectory 198 | cerr << "\n# Finished. Saving keyframe trajectory ..." << endl; 199 | ofstream towrite(se2clam::Config::WRITE_MAP_FILE_PATH + "/se2clam_kf_trajectory.txt"); 200 | vector vct = mpMap->getAllKF(); 201 | for (size_t i = 0; iisNull()){ 203 | Mat wTb = scv::inv(se2clam::Config::bTc * vct[i]->getPose()); 204 | Mat wRb = wTb.rowRange(0, 3).colRange(0, 3); 205 | g2o::Vector3D euler = g2o::internal::toEuler(se2clam::toMatrix3d(wRb)); 206 | towrite << vct[i]->id << " " << 207 | wTb.at(0, 3) << " " << 208 | wTb.at(1, 3) << " " << 209 | wTb.at(2, 3) << " " << 210 | euler(2) << endl; 211 | } 212 | } 213 | } 214 | 215 | void OdoSLAM::requestFinish() { 216 | unique_lock lock(mMutexFinish); 217 | mbFinishRequested = true; 218 | } 219 | 220 | bool OdoSLAM::checkFinish(){ 221 | unique_lock lock(mMutexFinish); 222 | if(se2clam::Config::LOCALIZATION_ONLY){ 223 | if(mpLocalizer->isFinished() || mpMapPub->isFinished()){ 224 | mbFinishRequested = true; 225 | return true; 226 | } 227 | } else { 228 | if(mpTrack->isFinished() || mpLocalMapper->isFinished() || 229 | mpGlobalMapper->isFinished() || mpMapPub->isFinished()) { 230 | mbFinishRequested = true; 231 | return true; 232 | } 233 | } 234 | 235 | return mbFinishRequested; 236 | } 237 | 238 | void OdoSLAM::sendRequestFinish(){ 239 | if (Config::LOCALIZATION_ONLY) { 240 | mpLocalizer->requestFinish(); 241 | mpMapPub->RequestFinish(); 242 | } 243 | else { 244 | mpTrack->requestFinish(); 245 | mpLocalMapper->requestFinish(); 246 | mpGlobalMapper->requestFinish(); 247 | mpMapPub->RequestFinish(); 248 | } 249 | } 250 | 251 | void OdoSLAM::checkAllExit() { 252 | if (Config::LOCALIZATION_ONLY) { 253 | while (1) { 254 | if (mpLocalizer->isFinished() && mpMapPub->isFinished()) 255 | break; 256 | else 257 | std::this_thread::sleep_for(std::chrono::microseconds(2)); 258 | } 259 | } 260 | else { 261 | while (1) { 262 | if (mpTrack->isFinished() && mpLocalMapper->isFinished() && 263 | mpGlobalMapper->isFinished() && mpMapPub->isFinished()) { 264 | break; 265 | } 266 | else { 267 | std::this_thread::sleep_for(std::chrono::microseconds(2)); 268 | } 269 | } 270 | } 271 | } 272 | 273 | void OdoSLAM::clear() { 274 | 275 | } 276 | 277 | void OdoSLAM::waitForFinish(){ 278 | while (1) { 279 | if (mbFinished) { 280 | break; 281 | } 282 | else { 283 | std::this_thread::sleep_for(std::chrono::microseconds(2)); 284 | } 285 | } 286 | std::this_thread::sleep_for(std::chrono::microseconds(20)); 287 | cerr << "wait for finish finished..." << endl; 288 | } 289 | 290 | bool OdoSLAM::ok(){ 291 | unique_lock lock(mMutexFinish); 292 | return !mbFinishRequested; 293 | } 294 | 295 | } // namespace se2clam 296 | -------------------------------------------------------------------------------- /src/Sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "Sensors.h" 6 | 7 | namespace se2clam { 8 | 9 | Sensors::Sensors() { 10 | img_updated = false; 11 | odo_updated = false; 12 | } 13 | 14 | Sensors::~Sensors() { 15 | 16 | } 17 | 18 | bool Sensors::update() { 19 | return odo_updated && img_updated; 20 | } 21 | 22 | void Sensors::updateImg(const cv::Mat &img_, double time_) 23 | { 24 | std::unique_lock lock(mutex_img); 25 | 26 | while(img_updated) 27 | { 28 | cndvSensorUpdate.wait(lock); 29 | } 30 | 31 | img_.copyTo(mImg); 32 | time_img = time_; 33 | img_updated = true; 34 | } 35 | 36 | void Sensors::updateOdo(double x_, double y_, double theta_, double time_) 37 | { 38 | std::unique_lock lock(mutex_odo); 39 | 40 | while(odo_updated) 41 | { 42 | cndvSensorUpdate.wait(lock); 43 | } 44 | mOdo.x = x_; 45 | mOdo.y = y_; 46 | mOdo.z = theta_; 47 | time_odo = time_; 48 | odo_updated = true; 49 | } 50 | 51 | void Sensors::readData(cv::Point3f& dataOdo, cv::Mat& dataImg){ 52 | std::unique_lock lock1(mutex_img); 53 | std::unique_lock lock2(mutex_odo); 54 | 55 | dataOdo = mOdo; 56 | mImg.copyTo(dataImg); 57 | 58 | odo_updated = false; 59 | img_updated = false; 60 | 61 | cndvSensorUpdate.notify_all(); 62 | } 63 | 64 | void Sensors::forceSetUpdate(bool val) 65 | { 66 | odo_updated = val; 67 | img_updated = val; 68 | } 69 | 70 | 71 | } 72 | -------------------------------------------------------------------------------- /src/converter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "converter.h" 6 | #include 7 | #include 8 | 9 | namespace se2clam{ 10 | 11 | 12 | Eigen::Vector2d toVector2d(const cv::Point2f &cvVector){ 13 | Eigen::Vector2d v; 14 | v << cvVector.x, cvVector.y; 15 | return v; 16 | } 17 | 18 | g2o::Isometry3D toIsometry3D(const cv::Mat &T){ 19 | Eigen::Matrix R; 20 | R << T.at(0,0), T.at(0,1), T.at(0,2), 21 | T.at(1,0), T.at(1,1), T.at(1,2), 22 | T.at(2,0), T.at(2,1), T.at(2,2); 23 | g2o::Isometry3D ret = (g2o::Isometry3D) Eigen::Quaterniond(R); 24 | Eigen::Vector3d t(T.at(0,3), T.at(1,3), T.at(2,3)); 25 | ret.translation() = t; 26 | return ret; 27 | } 28 | 29 | cv::Mat toCvMat(const g2o::Isometry3D &t){ 30 | return toCvMat(g2o::internal::toSE3Quat(t)); 31 | } 32 | 33 | cv::Point2f toCvPt2f(const Eigen::Vector2d& vec){ 34 | return cv::Point2f(vec(0),vec(1)); 35 | } 36 | 37 | cv::Point3f toCvPt3f(const Eigen::Vector3d& vec){ 38 | return cv::Point3f(vec(0), vec(1), vec(2)); 39 | } 40 | 41 | g2o::Isometry3D toIsometry3D(const g2o::SE3Quat &se3quat){ 42 | return g2o::internal::fromSE3Quat(se3quat); 43 | } 44 | 45 | g2o::SE3Quat toSE3Quat(const g2o::Isometry3D &iso){ 46 | return g2o::internal::toSE3Quat(iso); 47 | } 48 | 49 | 50 | cv::Mat toCvMat6f(const g2o::Matrix6d& m) { 51 | cv::Mat mat(6, 6, CV_32FC1); 52 | for(int i = 0; i < 6; i++) 53 | for(int j = 0; j < 6; j++) 54 | mat.at(i,j) = (float)m(i,j); 55 | return mat; 56 | } 57 | 58 | g2o::Matrix6d toMatrix6d(const cv::Mat &cvMat6d){ 59 | g2o::Matrix6d m = g2o::Matrix6d::Zero(); 60 | for(int i = 0; i < 6; i++) 61 | for(int j = 0; j < 6; j++) 62 | m(i,j) = cvMat6d.at(i,j); 63 | return m; 64 | } 65 | 66 | 67 | // below from ORB_SLAM: https://github.com/raulmur/ORB_SLAM 68 | std::vector toDescriptorVector(const cv::Mat &Descriptors) 69 | { 70 | std::vector vDesc; 71 | vDesc.reserve(Descriptors.rows); 72 | for (int j=0;j R; 81 | R << cvT.at(0,0), cvT.at(0,1), cvT.at(0,2), 82 | cvT.at(1,0), cvT.at(1,1), cvT.at(1,2), 83 | cvT.at(2,0), cvT.at(2,1), cvT.at(2,2); 84 | 85 | Eigen::Matrix t(cvT.at(0,3), cvT.at(1,3), cvT.at(2,3)); 86 | 87 | return g2o::SE3Quat(R,t); 88 | } 89 | 90 | cv::Mat toCvMat(const g2o::SE3Quat &SE3) 91 | { 92 | Eigen::Matrix eigMat = SE3.to_homogeneous_matrix(); 93 | return toCvMat(eigMat); 94 | } 95 | 96 | 97 | cv::Mat toCvMat(const Eigen::Matrix &m) 98 | { 99 | cv::Mat cvMat(4,4,CV_32FC1); 100 | for(int i=0;i<4;i++) 101 | for(int j=0; j<4; j++) 102 | cvMat.at(i,j)=m(i,j); 103 | 104 | return cvMat.clone(); 105 | } 106 | 107 | 108 | cv::Mat toCvMat(const Eigen::Matrix3d &m) 109 | { 110 | cv::Mat cvMat(3,3,CV_32FC1); 111 | for(int i=0;i<3;i++) 112 | for(int j=0; j<3; j++) 113 | cvMat.at(i,j)=m(i,j); 114 | 115 | return cvMat.clone(); 116 | } 117 | 118 | cv::Mat toCvMat(const Eigen::Matrix &m) 119 | { 120 | cv::Mat cvMat(3,1,CV_32FC1); 121 | for(int i=0;i<3;i++) 122 | cvMat.at(i)=m(i); 123 | 124 | return cvMat.clone(); 125 | } 126 | 127 | cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t) 128 | { 129 | cv::Mat cvMat = cv::Mat::eye(4,4,CV_32FC1); 130 | for(int i=0;i<3;i++) 131 | { 132 | for(int j=0;j<3;j++) 133 | { 134 | cvMat.at(i,j)=R(i,j); 135 | } 136 | } 137 | for(int i=0;i<3;i++) 138 | { 139 | cvMat.at(i,3)=t(i); 140 | } 141 | 142 | return cvMat.clone(); 143 | } 144 | 145 | Eigen::Matrix toVector3d(const cv::Mat &cvVector) 146 | { 147 | Eigen::Matrix v; 148 | v << cvVector.at(0), cvVector.at(1), cvVector.at(2); 149 | 150 | return v; 151 | } 152 | 153 | Eigen::Matrix toVector3d(const cv::Point3f &cvPoint) 154 | { 155 | Eigen::Matrix v; 156 | v << cvPoint.x, cvPoint.y, cvPoint.z; 157 | 158 | return v; 159 | } 160 | 161 | Eigen::Matrix toVector2d(const cv::Mat &cvVector) 162 | { 163 | assert((cvVector.rows==2 && cvVector.cols==1) || (cvVector.cols==2 && cvVector.rows==1)); 164 | 165 | Eigen::Matrix v; 166 | v << cvVector.at(0), cvVector.at(1); 167 | 168 | return v; 169 | } 170 | 171 | Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3) 172 | { 173 | Eigen::Matrix M; 174 | 175 | M << cvMat3.at(0,0), cvMat3.at(0,1), cvMat3.at(0,2), 176 | cvMat3.at(1,0), cvMat3.at(1,1), cvMat3.at(1,2), 177 | cvMat3.at(2,0), cvMat3.at(2,1), cvMat3.at(2,2); 178 | 179 | return M; 180 | } 181 | 182 | std::vector toQuaternion(const cv::Mat &M) 183 | { 184 | Eigen::Matrix eigMat = toMatrix3d(M); 185 | Eigen::Quaterniond q(eigMat); 186 | 187 | std::vector v(4); 188 | 189 | v[0] = q.w(); 190 | v[1] = q.x(); 191 | v[2] = q.y(); 192 | v[3] = q.z(); 193 | 194 | return v; 195 | } 196 | 197 | 198 | } 199 | -------------------------------------------------------------------------------- /src/sparsifier.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include "sparsifier.h" 6 | 7 | using namespace std; 8 | 9 | Sparsifier::Sparsifier() 10 | { 11 | } 12 | 13 | 14 | // compute Hessian from edge XYZ2UV 15 | void Sparsifier::HessianXYZ2UV(g2o::SE3Quat KF, g2o::Vector3D MP, MeasXYZ2UV measure, 16 | g2o::CameraParameters* pCamParam, 17 | Eigen::Matrix & H ) 18 | { 19 | // ... 20 | } 21 | 22 | // compute Jacobian matrix of image measurement (UV) w.r.t. KF (SE3) and MP (XYZ) 23 | // state vector of KF (SE3) is defined as (x;y;z;qx;qy;qz), from toMinimalVector() 24 | void Sparsifier::JacobianXYZ2UV(g2o::SE3Quat KF, g2o::Vector3D MP, g2o::CameraParameters* pCamParam, 25 | Eigen::Matrix & J) 26 | { 27 | g2o::Vector2D z_ref = pCamParam->cam_map(KF.map(MP)); 28 | g2o::Vector6d v6KF = KF.toMinimalVector(); 29 | 30 | for (int i=0; i<6; i++) { 31 | g2o::Vector6d v6KF_delta = v6KF; 32 | v6KF_delta[i] += 0.000001; 33 | 34 | g2o::SE3Quat KF_delta; 35 | KF_delta.fromMinimalVector(v6KF_delta); 36 | 37 | g2o::Vector2D z_delta = pCamParam->cam_map(KF_delta.map(MP)); 38 | g2o::Vector2D dz = z_delta - z_ref; 39 | 40 | J(i,0) = dz(0)/0.000001; 41 | J(i,1) = dz(1)/0.000001; 42 | } 43 | 44 | for (int i=0; i<3; i++) { 45 | Eigen::Vector3d MP_delta = MP; 46 | MP_delta[i] += 0.000001; 47 | 48 | g2o::Vector2D z_delta = pCamParam->cam_map(KF.map(MP_delta)); 49 | g2o::Vector2D dz = z_delta - z_ref; 50 | 51 | J(i+6,0) = dz(0)/0.000001; 52 | J(i+6,1) = dz(1)/0.000001; 53 | } 54 | } 55 | 56 | // compute Jacobian matrix of edge SE3XYZ 57 | void Sparsifier::JacobianSE3XYZ(const g2o::SE3Quat KF, const g2o::Vector3D MP, 58 | Eigen::Matrix & J) 59 | { 60 | g2o::Vector3D z_ref = KF.inverse() * MP; 61 | g2o::Vector6d v6KF = KF.toMinimalVector(); 62 | double delta = 1e-6; 63 | 64 | g2o::Vector3D z_delta; 65 | g2o::Vector3D dz; 66 | 67 | for (int i=0; i<9; i++) { 68 | 69 | if (i<6) { 70 | g2o::Vector6d v6KF_delta = v6KF; 71 | v6KF_delta[i] += delta; 72 | 73 | g2o::SE3Quat KF_delta; 74 | KF_delta.fromMinimalVector(v6KF_delta); 75 | 76 | z_delta = KF_delta.inverse() * MP; 77 | dz = z_delta - z_ref; 78 | } 79 | else { 80 | Eigen::Vector3d MP_delta = MP; 81 | MP_delta[i-6] += delta; 82 | 83 | z_delta = KF.inverse() * MP_delta; 84 | dz = z_delta - z_ref; 85 | } 86 | 87 | J(0,i) = dz(0)/delta; 88 | J(1,i) = dz(1)/delta; 89 | J(2,i) = dz(2)/delta; 90 | } 91 | } 92 | 93 | // compute Hessian matrix of edge SE3XYZ 94 | void Sparsifier::HessianSE3XYZ(const g2o::SE3Quat KF, const g2o::Vector3D MP, const g2o::Matrix3D info, 95 | Eigen::Matrix & H) 96 | { 97 | Eigen::Matrix J; 98 | JacobianSE3XYZ(KF, MP, J); 99 | H = J.transpose() * info * J; 100 | } 101 | 102 | // do marginalize and return SE3 constraint between KFs 103 | void Sparsifier::DoMarginalizeSE3XYZ(const std::vector > & vKF, const std::vector > & vMP, 104 | const std::vector > & vMeasure, 105 | g2o::SE3Quat & z_out, g2o::Matrix6d & info_out) 106 | { 107 | // int idxKF1, idxKF2; 108 | // idxKF1 = 0; idxKF2 = 1; 109 | 110 | vector vIdMP; 111 | vector vMeasureRelated; 112 | 113 | int numMeas = vMeasure.size(); 114 | for (int i=0; i::iterator itr; 139 | itr = find(vIdMP.begin(), vIdMP.end(), idMP); 140 | int idMPBlk = itr - vIdMP.begin(); 141 | 142 | g2o::Matrix3D info = measure.info; 143 | 144 | // DEBUG ON NAN 145 | double d = info(0,0); 146 | if(std::isnan(d)) { 147 | cerr << "ERROR!!!" << endl; 148 | } 149 | 150 | Eigen::Matrix H_local; 151 | 152 | HessianSE3XYZ(vKF.at(idKF), vMP.at(idMPBlk), info, H_local); 153 | 154 | H.block(idKF*6,idKF*6,6,6) += H_local.block(0,0,6,6); 155 | H.block(12+3*idMPBlk,12+3*idMPBlk,3,3) += H_local.block(6,6,3,3); 156 | H.block(idKF*6,12+3*idMPBlk,6,3) += H_local.block(0,6,6,3); 157 | H.block(12+3*idMPBlk,idKF*6,3,6) += H_local.block(6,0,3,6); 158 | 159 | } 160 | 161 | H.block(0,0,12,12) += Eigen::MatrixXd::Identity(12,12) * 1e-6; 162 | 163 | // marginalize hessian 164 | 165 | Eigen::MatrixXd H11 = H.block(0,0,12,12); 166 | Eigen::MatrixXd H12 = H.block(0,12,12,dim-12); 167 | Eigen::MatrixXd H21 = H.block(12,0,dim-12,12); 168 | Eigen::MatrixXd H22 = H.block(12,12,dim-12,dim-12); 169 | 170 | Eigen::MatrixXd T = H22.ldlt().solve(H21); 171 | Eigen::MatrixXd H_marginal = H11 - H12*T; 172 | 173 | InfoSE3(vKF[0], vKF[1], H_marginal, info_out); 174 | z_out = vKF[0].inverse() * vKF[1]; 175 | } 176 | 177 | void Sparsifier::JacobianSE3(const g2o::SE3Quat KF1, const g2o::SE3Quat KF2, 178 | Eigen::Matrix & J) 179 | { 180 | g2o::SE3Quat z_se3_ref = KF1.inverse() * KF2; 181 | g2o::Vector6d z_ref = z_se3_ref.toMinimalVector(); 182 | 183 | g2o::Vector6d v6_KF1 = KF1.toMinimalVector(); 184 | g2o::Vector6d v6_KF2 = KF2.toMinimalVector(); 185 | 186 | double delta = 1e-6; 187 | 188 | g2o::Vector6d z_delta; 189 | g2o::Vector6d dz; 190 | 191 | for (int i=0; i<12; i++) { 192 | if (i<6) { 193 | g2o::Vector6d v6_KF1_delta = v6_KF1; 194 | v6_KF1_delta[i] += delta; 195 | 196 | g2o::SE3Quat se3_KF1_delta; 197 | se3_KF1_delta.fromMinimalVector(v6_KF1_delta); 198 | 199 | z_delta = (se3_KF1_delta.inverse()*KF2).toMinimalVector(); 200 | dz = z_delta - z_ref; 201 | } 202 | else { 203 | g2o::Vector6d v6_KF2_delta = v6_KF2; 204 | v6_KF2_delta[i-6] += delta; 205 | 206 | g2o::SE3Quat se3_KF2_delta; 207 | se3_KF2_delta.fromMinimalVector(v6_KF2_delta); 208 | 209 | z_delta = (KF1.inverse()*se3_KF2_delta).toMinimalVector(); 210 | dz = z_delta - z_ref; 211 | } 212 | 213 | J.col(i) = dz/delta; 214 | } 215 | } 216 | 217 | void Sparsifier::InfoSE3(const g2o::SE3Quat KF1, const g2o::SE3Quat KF2, const Eigen::Matrix & H, 218 | Eigen::Matrix & I) 219 | { 220 | Eigen::Matrix J; 221 | JacobianSE3(KF1, KF2, J); 222 | 223 | // cerr << H.eigenvalues() << endl; 224 | // Eigen::Matrix H_inv = H.inverse(); 225 | // cerr << H_inv.eigenvalues() << endl; 226 | // Eigen::Matrix I_inv = J * H.inverse() * J.transpose(); 227 | // cerr << I_inv.eigenvalues() << endl; 228 | // Eigen::Matrix I = I_inv.inverse(); 229 | // cerr << I.eigenvalues() << endl; 230 | 231 | I = (J * H.inverse() * J.transpose()).inverse(); 232 | 233 | // Set symmetric 234 | I = (I + I.transpose())/2; 235 | 236 | // Compute SVD 237 | Eigen::JacobiSVD svd(I, Eigen::ComputeFullU | Eigen::ComputeFullV); 238 | Eigen::Matrix s = svd.singularValues(); 239 | Eigen::Matrix U = svd.matrixU(); 240 | Eigen::Matrix V = svd.matrixV(); 241 | 242 | for (int i=0; i<6; i++) { 243 | Eigen::Matrix ui = U.block<6,1>(0,i); 244 | Eigen::Matrix vi = V.block<6,1>(0,i); 245 | 246 | double norm = ui.dot(vi); 247 | if (norm >= 0) { 248 | s(i,0) = max(s(i,0), 1e-6); 249 | s(i,0) = min(s(i,0), 1e4); 250 | } 251 | else { 252 | s(i,0) = -1e-6; 253 | } 254 | } 255 | 256 | // Set limit on eigenvalues and refine info matrix 257 | Eigen::Matrix S = Eigen::MatrixXd::Zero(6,6); 258 | for (int i=0; i<6; i++) { 259 | S(i,i) = s(i,0); 260 | } 261 | I = U*S*(V.transpose()); 262 | 263 | // cerr << "S:" << endl << S << endl; 264 | // cerr << "U:" << endl << U << endl; 265 | // cerr << "V:" << endl << V << endl; 266 | // cerr << "I:" << endl << I << endl; 267 | 268 | // Set symmetric 269 | I = (I + I.transpose())/2; 270 | 271 | // cerr << "I_final:" << endl << I << endl; 272 | } 273 | 274 | 275 | 276 | 277 | 278 | -------------------------------------------------------------------------------- /src/sugarCV.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // This file is part of se2clam. 3 | // Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | /// 5 | 6 | #include "sugarCV.h" 7 | #include 8 | #include 9 | namespace scv{ 10 | 11 | using namespace cv; 12 | using namespace std; 13 | 14 | Mat inv(const Mat &T4x4){ 15 | assert(T4x4.cols == 4 && T4x4.rows == 4); 16 | Mat RT = T4x4.rowRange(0,3).colRange(0,3).t(); 17 | Mat t = -RT * T4x4.rowRange(0,3).col(3); 18 | Mat T = Mat::eye(4,4,CV_32FC1); 19 | RT.copyTo(T.rowRange(0,3).colRange(0,3)); 20 | t.copyTo(T.rowRange(0,3).col(3)); 21 | return T; 22 | } 23 | 24 | void pts2Ftrs(const vector& _orgnFtrs, const vector& _points, vector& _features) { 25 | _features.resize(_points.size()); 26 | for (size_t i = 0; i < _points.size(); i ++) { 27 | _features[i] = _orgnFtrs[i]; 28 | _features[i].pt = _points[i]; 29 | } 30 | } 31 | 32 | 33 | Mat drawKeysWithNum(const vector keys, const Mat &img, vector mask){ 34 | if (mask.size() == 0) { 35 | mask = vector(keys.size(), true); 36 | } 37 | Mat out = img.clone(); 38 | if (img.channels() == 1) 39 | cvtColor(img, out, CV_GRAY2BGR); 40 | for (unsigned i = 0; i < mask.size(); i++) { 41 | if (!mask[i]) 42 | continue; 43 | Point2f pt1 = keys[i].pt; 44 | circle(out, pt1, 3, Scalar(0, 0, 200), 2); 45 | putText(out, std::to_string(i), pt1+Point2f(2,-2), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255,255,255)); 46 | } 47 | return out.clone(); 48 | } 49 | 50 | 51 | 52 | Mat sk_sym(const Point3f _v){ 53 | Mat mat(3,3,CV_32FC1, Scalar(0)); 54 | mat.at(0,1) = -_v.z; 55 | mat.at(0,2) = _v.y; 56 | mat.at(1,0) = _v.z; 57 | mat.at(1,2) = -_v.x; 58 | mat.at(2,0) = -_v.y; 59 | mat.at(2,1) = _v.x; 60 | return mat; 61 | } 62 | 63 | 64 | Point3f triangulate(const Point2f &pt1, const Point2f &pt2, const Mat &P1, const Mat &P2){ 65 | Mat A(4,4,CV_32FC1); 66 | 67 | A.row(0) = pt1.x*P1.row(2)-P1.row(0); 68 | A.row(1) = pt1.y*P1.row(2)-P1.row(1); 69 | A.row(2) = pt2.x*P2.row(2)-P2.row(0); 70 | A.row(3) = pt2.y*P2.row(2)-P2.row(1); 71 | 72 | Mat u, w, vt, x3D; 73 | SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A|SVD::FULL_UV); 74 | x3D = vt.row(3).t(); 75 | x3D = x3D.rowRange(0,3)/x3D.at(3); 76 | 77 | return Point3f(x3D); 78 | 79 | // Eigen::Matrix4d A; 80 | // Mtrx34d _P1, _P2; 81 | // for(int i = 0; i < 3; i++) 82 | // for(int j = 0; j < 4; j++) { 83 | // _P1(i,j) = P1.at(i,j); 84 | // _P2(i,j) = P2.at(i,j); 85 | // } 86 | 87 | 88 | // A.row(0).noalias() = pt1.x*_P1.row(2)-_P1.row(0); 89 | // A.row(1).noalias() = pt1.y*_P1.row(2)-_P1.row(1); 90 | // A.row(2).noalias() = pt2.x*_P2.row(2)-_P2.row(0); 91 | // A.row(3).noalias() = pt2.y*_P2.row(2)-_P2.row(1); 92 | 93 | // Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); 94 | // Eigen::Matrix4d V = svd.matrixV(); 95 | // Vtr3d x3d = (1.0/V(3,3))*V.block<3,1>(0,3); 96 | 97 | // x3D = Mat(3,1,CV_32FC1); 98 | // for(int i = 0;i < 3; i++) 99 | // x3D.at(i) = x3d(i); 100 | } 101 | 102 | Point2f camprjc(const Mat &_K, const Point3f &_pt) 103 | { 104 | Point3f uvw = Matx33f(_K) * _pt; 105 | return Point2f(uvw.x/uvw.z, uvw.y/uvw.z); 106 | } 107 | 108 | bool checkParallax(const Point3f &o1, const Point3f &o2, const Point3f &pt3, int minDegree){ 109 | float minCos[4] = {0.9998, 0.9994, 0.9986, 0.9976}; 110 | Point3f p1 = pt3 - o1; 111 | Point3f p2 = pt3 - o2; 112 | float cosParallax = cv::norm(p1.dot(p2)) / ( cv::norm(p1) * cv::norm(p2) ); 113 | return cosParallax < minCos[minDegree-1]; 114 | } 115 | 116 | Point3f se3map(const Mat &_Tcw, const Point3f &_pt) 117 | { 118 | Matx33f R(_Tcw.rowRange(0,3).colRange(0,3)); 119 | Point3f t(_Tcw.rowRange(0,3).col(3)); 120 | return (R*_pt + t); 121 | } 122 | 123 | 124 | } // namespace scv 125 | -------------------------------------------------------------------------------- /test/datapub.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace cv; 12 | using namespace std; 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc, argv, "datapub"); 17 | ros::start(); 18 | 19 | const char* ImgTopic = "/camera/image_gray"; 20 | const char* OdoTopic = "/odo_raw"; 21 | 22 | std::string path = argv[1]; 23 | int N = atoi(argv[2]); // Number of images 24 | 25 | string fullOdoName = path + "/odo_raw.txt"; 26 | ifstream rec(fullOdoName); 27 | float x,y,theta; 28 | string line; 29 | 30 | ros::NodeHandle nh; 31 | image_transport::ImageTransport it(nh); 32 | image_transport::Publisher img_pub = it.advertise(ImgTopic, 1); 33 | ros::Publisher odo_pub = nh.advertise(OdoTopic, 100); 34 | 35 | 36 | ros::Rate rate(30); 37 | for(int i = 0; i < N && ros::ok(); i++) 38 | { 39 | string fullImgName = path + "/image/" + to_string(i) + ".bmp"; 40 | Mat img = imread(fullImgName, CV_LOAD_IMAGE_GRAYSCALE); 41 | getline(rec, line); 42 | istringstream iss(line); 43 | iss >> x >> y >> theta; 44 | sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg(); 45 | geometry_msgs::Vector3Stamped odo_msg; 46 | 47 | img_msg->header.stamp = ros::Time::now(); 48 | odo_msg.header.stamp = img_msg->header.stamp; 49 | odo_msg.vector.x = x; 50 | odo_msg.vector.y = y; 51 | odo_msg.vector.z = theta; 52 | 53 | img_pub.publish(img_msg); 54 | odo_pub.publish(odo_msg); 55 | rate.sleep(); 56 | } 57 | 58 | ros::shutdown(); 59 | return 0; 60 | 61 | } 62 | -------------------------------------------------------------------------------- /test/imgview.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of se2clam. 3 | * Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | */ 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | void imageCallback(const sensor_msgs::ImageConstPtr& msg) 11 | { 12 | try 13 | { 14 | cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); 15 | cv::waitKey(30); 16 | } 17 | catch (cv_bridge::Exception& e) 18 | { 19 | ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); 20 | } 21 | } 22 | 23 | 24 | int main(int argc, char **argv) 25 | { 26 | //! Initialize 27 | ros::init(argc, argv, "imgview"); 28 | ros::start(); 29 | 30 | ros::NodeHandle nh; 31 | 32 | cv::namedWindow("view", cv::WINDOW_NORMAL); 33 | //cv::startWindowThread(); 34 | image_transport::ImageTransport it(nh); 35 | image_transport::Subscriber sub = it.subscribe("/camera/framepub", 1, imageCallback); 36 | ros::spin(); 37 | cv::destroyWindow("view"); 38 | 39 | return 0; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /test/test_ros.cpp: -------------------------------------------------------------------------------- 1 | // /** 2 | // This file is part of se2clam. 3 | // Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | /// 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "OdoSLAM.h" 15 | 16 | 17 | using namespace std; 18 | using namespace cv; 19 | using namespace Eigen; 20 | 21 | class SensorHandler{ 22 | public: 23 | SensorHandler(se2clam::OdoSLAM* slam){ 24 | _slam = slam; 25 | } 26 | 27 | ~SensorHandler(){} 28 | 29 | inline void updateImg(const sensor_msgs::ImageConstPtr& msg) 30 | { 31 | _slam->receiveImgData(cv_bridge::toCvShare(msg, "mono8")->image.clone()); 32 | } 33 | 34 | inline void updateOdo(const geometry_msgs::Vector3Stamped& msg) 35 | { 36 | _slam->receiveOdoData(msg.vector.x, msg.vector.y, msg.vector.z); 37 | } 38 | 39 | private: 40 | se2clam::OdoSLAM* _slam; 41 | 42 | }; 43 | 44 | geometry_msgs::Pose toRosPose(const cv::Mat T4x4) 45 | { 46 | geometry_msgs::Pose rosPose; 47 | Eigen::Matrix eigMat = se2clam::toMatrix3d(T4x4.rowRange(0,3).colRange(0,3)); 48 | Eigen::Quaterniond quaterd(eigMat); 49 | rosPose.position.x = T4x4.at(0,3); 50 | rosPose.position.y = T4x4.at(1,3); 51 | rosPose.position.z = T4x4.at(2,3); 52 | rosPose.orientation.w = quaterd.w(); 53 | rosPose.orientation.x = quaterd.x(); 54 | rosPose.orientation.y = quaterd.y(); 55 | rosPose.orientation.z = quaterd.z(); 56 | return rosPose; 57 | } 58 | 59 | int main(int argc, char **argv) 60 | { 61 | 62 | //! Initialize 63 | ros::init(argc, argv, "test_ros"); 64 | ros::start(); 65 | 66 | if(argc != 3){ 67 | cerr << "Input data_path and PATH_TO_ORBvoc.bin!" << endl; 68 | ros::shutdown(); 69 | return 1; 70 | } 71 | 72 | 73 | se2clam::OdoSLAM system; 74 | 75 | SensorHandler sensor(&system); 76 | 77 | string path = argv[1]; 78 | string strVoc = argv[2]; 79 | system.setVocFileBin(strVoc.c_str()); 80 | system.setDataPath(path.c_str()); 81 | 82 | system.start(); 83 | 84 | ros::NodeHandle nh; 85 | image_transport::ImageTransport it(nh); 86 | image_transport::Subscriber img_sub = it.subscribe("/camera/image_gray", 1, &SensorHandler::updateImg, &sensor); 87 | ros::Subscriber odo_sub = nh.subscribe("/odo_raw", 1, &SensorHandler::updateOdo, &sensor); 88 | ros::Publisher posev_pub = nh.advertise("/vehicle_pose_w_b", 1); 89 | ros::Publisher posec_pub = nh.advertise("/vehicle_pose_w_c", 1); 90 | 91 | while(nh.ok() && system.ok()) { 92 | 93 | geometry_msgs::PoseStamped pose_wb_stamped, pose_wc_stamped; 94 | pose_wb_stamped.pose = toRosPose(system.getCurrentVehiclePose()); 95 | pose_wb_stamped.header.stamp = ros::Time::now(); 96 | pose_wc_stamped.pose = toRosPose(system.getCurrentCameraPoseWC()); 97 | pose_wc_stamped.header.stamp = pose_wb_stamped.header.stamp; 98 | posev_pub.publish(pose_wb_stamped); 99 | posec_pub.publish(pose_wc_stamped); 100 | 101 | ros::spinOnce(); 102 | 103 | } 104 | 105 | cerr << "Exit system.. " << endl; 106 | 107 | system.waitForFinish(); 108 | 109 | ros::shutdown(); 110 | 111 | return 0; 112 | } 113 | 114 | 115 | -------------------------------------------------------------------------------- /test/test_vn.cpp: -------------------------------------------------------------------------------- 1 | // /** 2 | // This file is part of se2clam. 3 | // Copyright (C) Fan ZHENG (github.com/izhengfan), Hengbo TANG (github.com/hbtang) 4 | /// 5 | 6 | #include "OdoSLAM.h" 7 | #include 8 | 9 | using namespace std; 10 | using namespace cv; 11 | using namespace Eigen; 12 | 13 | int main(int argc, char **argv) 14 | { 15 | //! Initialize 16 | ros::init(argc, argv, "test_vn"); 17 | ros::start(); 18 | 19 | if(argc != 3){ 20 | cerr << "Usage: rosrun se2clam test_vn dataPath PATH_TO_ORBvoc.bin" << endl; 21 | ros::shutdown(); 22 | return 1; 23 | } 24 | 25 | se2clam::OdoSLAM system; 26 | 27 | system.setVocFileBin(argv[2]); 28 | system.setDataPath(argv[1]); 29 | system.start(); 30 | 31 | string fullOdoName = se2clam::Config::DataPath + "/odo_raw.txt"; 32 | ifstream rec(fullOdoName); 33 | float x,y,theta; 34 | string line; 35 | 36 | ros::Rate rate(se2clam::Config::FPS); 37 | 38 | int n = se2clam::Config::ImgIndex; 39 | int i = 0; 40 | 41 | for(; i < n && system.ok(); i++) { 42 | 43 | string fullImgName = se2clam::Config::DataPath + "/image/" + to_string(i) + ".bmp"; 44 | Mat img = imread(fullImgName, CV_LOAD_IMAGE_GRAYSCALE); 45 | std::getline(rec, line); 46 | istringstream iss(line); 47 | iss >> x >> y >> theta; 48 | 49 | system.receiveOdoData(x, y, theta); 50 | system.receiveImgData(img); 51 | 52 | rate.sleep(); 53 | } 54 | cerr << "Finish test..." << endl; 55 | 56 | system.requestFinish(); 57 | system.waitForFinish(); 58 | 59 | ros::shutdown(); 60 | 61 | cerr << "Rec close..." << endl; 62 | rec.close(); 63 | cerr << "Exit test..." << endl; 64 | return 0; 65 | 66 | } 67 | 68 | --------------------------------------------------------------------------------