├── .gitignore ├── CMakeLists.txt ├── CMakeModules ├── FindBLAS.cmake ├── FindCSparse.cmake ├── FindCholmod.cmake ├── FindEigen.cmake ├── FindG2O.cmake ├── FindLAPACK.cmake └── FindSuiteSparse.cmake ├── LICENSE ├── README.md ├── bin ├── EuRoC.yaml ├── MH01.txt ├── test_live_vo ├── test_pipel_euroc └── test_pipeline ├── include ├── sophus │ ├── average.hpp │ ├── common.hpp │ ├── example_ensure_handler.cpp │ ├── formatstring.hpp │ ├── geometry.hpp │ ├── interpolate.hpp │ ├── interpolate_details.hpp │ ├── num_diff.hpp │ ├── rotation_matrix.hpp │ ├── rxso2.hpp │ ├── rxso3.hpp │ ├── se2.hpp │ ├── se3.hpp │ ├── sim2.hpp │ ├── sim3.hpp │ ├── sim_details.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── test_macros.hpp │ ├── types.hpp │ └── velocities.hpp └── svo │ ├── blender_utils.h │ ├── bundle_adjustment.h │ ├── camera_model.h │ ├── config.h │ ├── corner_10.h │ ├── debug.h │ ├── depth_filter.h │ ├── fast.h │ ├── faster_corner_utilities.h │ ├── feature.h │ ├── feature_alignment.h │ ├── feature_detection.h │ ├── frame.h │ ├── frame_handler_base.h │ ├── frame_handler_mono.h │ ├── global.h │ ├── homography.h │ ├── initialization.h │ ├── map.h │ ├── matcher.h │ ├── math_lib.h │ ├── patch_score.h │ ├── point.h │ ├── pose_optimizer.h │ ├── reprojector.h │ ├── ringbuffer.h │ ├── robust_cost.h │ ├── slamviewer.h │ ├── sparse_align.h │ └── sparse_img_align.h ├── lib └── libsvo.so ├── src ├── bundle_adjustment.cpp ├── camera_model.cpp ├── config.cpp ├── debug.cpp ├── depth_filter.cpp ├── fast_10.cpp ├── fast_10_score.cpp ├── fast_dect.cpp ├── fast_nonmax_3x3.cpp ├── faster_corner_10_sse.cpp ├── feature_alignment.cpp ├── feature_detection.cpp ├── frame.cpp ├── frame_handler_base.cpp ├── frame_handler_mono.cpp ├── homography.cpp ├── initialization.cpp ├── map.cpp ├── matcher.cpp ├── math_utils.cpp ├── point.cpp ├── pose_optimizer.cpp ├── reprojector.cpp ├── robust_cost.cpp ├── slamviewer.cpp ├── sparse_align.cpp └── sparse_img_align.cpp └── test ├── test_live_vo.cpp ├── test_pipel_euroc.cpp └── test_pipeline.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # user build settings 3 | 4 | SET(TRACE TRUE) 5 | SET(HAVE_G2O false) #TRUE 6 | 7 | SET(DEBUG_OUTPUT TRUE) # Only relevant if build without ROS 8 | 9 | ################################################################################ 10 | 11 | SET(PROJECT_NAME svo_edgelete_live) 12 | PROJECT(${PROJECT_NAME}) 13 | CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3) 14 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo , Debug 15 | SET(CMAKE_VERBOSE_MAKEFILE OFF) 16 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/") 17 | 18 | # Set definitions 19 | IF(TRACE) 20 | ADD_DEFINITIONS(-DSVO_TRACE) 21 | ENDIF() 22 | 23 | 24 | IF(DEBUG_OUTPUT) 25 | ADD_DEFINITIONS(-DSVO_DEBUG_OUTPUT) 26 | ENDIF() 27 | 28 | # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid 29 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 30 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 31 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 32 | ELSE() 33 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 34 | ENDIF() 35 | IF(CMAKE_COMPILER_IS_GNUCC) 36 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 37 | ELSE() 38 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 39 | ENDIF() 40 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 41 | 42 | # Add plain cmake packages 43 | FIND_PACKAGE(OpenCV REQUIRED) 44 | FIND_PACKAGE(Eigen REQUIRED) 45 | FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system) 46 | 47 | find_package(Pangolin REQUIRED) 48 | 49 | #FIND_PACKAGE(fast REQUIRED) 50 | #FIND_PACKAGE(vikit_common REQUIRED) 51 | SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 52 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 53 | 54 | 55 | # Include dirs 56 | INCLUDE_DIRECTORIES( 57 | include 58 | ${Eigen_INCLUDE_DIRS} 59 | ${OpenCV_INCLUDE_DIRS} 60 | ${Boost_INCLUDE_DIRS} 61 | ${Pangolin_INCLUDE_DIRS} 62 | ) 63 | 64 | # Set link libraries 65 | LIST(APPEND LINK_LIBS 66 | ${OpenCV_LIBS} 67 | ${Boost_LIBRARIES} 68 | ) 69 | 70 | 71 | # Set sourcefiles 72 | LIST(APPEND SOURCEFILES 73 | src/frame_handler_mono.cpp 74 | src/frame_handler_base.cpp 75 | src/frame.cpp 76 | src/point.cpp 77 | src/map.cpp 78 | src/pose_optimizer.cpp 79 | src/initialization.cpp 80 | src/matcher.cpp 81 | src/reprojector.cpp 82 | src/feature_alignment.cpp 83 | src/feature_detection.cpp 84 | src/depth_filter.cpp 85 | src/config.cpp 86 | 87 | src/camera_model.cpp 88 | src/sparse_align.cpp 89 | src/debug.cpp 90 | src/math_utils.cpp 91 | src/homography.cpp 92 | src/robust_cost.cpp 93 | src/fast_10_score.cpp 94 | src/fast_nonmax_3x3.cpp 95 | src/fast_10.cpp 96 | src/faster_corner_10_sse.cpp 97 | 98 | src/slamviewer.cpp 99 | ) 100 | 101 | IF(HAVE_G2O) 102 | ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT) 103 | ENDIF() 104 | # Add g2o if available 105 | IF(HAVE_G2O) 106 | FIND_PACKAGE(G2O REQUIRED) 107 | 108 | message("g2o is found: ${G2O_INCLUDE_DIR}") 109 | 110 | INCLUDE_DIRECTORIES( 111 | ${G2O_INCLUDE_DIR} 112 | /usr/include/suitesparse # for cholmod 113 | ) 114 | 115 | #LIST(APPEND LINK_LIBS 116 | # ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE} 117 | # ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse 118 | #) 119 | LIST(APPEND LINK_LIBS g2o_core_d g2o_solver_csparse_d g2o_csparse_extension_d g2o_types_sba_d g2o_solver_dense_d g2o_stuff_d g2o_parser_d g2o_solver_pcg_d cholmod cxsparse ) 120 | LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp) 121 | ENDIF(HAVE_G2O) 122 | 123 | # Create svo library 124 | ADD_LIBRARY(svo SHARED ${SOURCEFILES}) 125 | TARGET_LINK_LIBRARIES(svo ${LINK_LIBS} ${Pangolin_LIBRARIES}) 126 | 127 | ################################################################################ 128 | # TESTS 129 | 130 | ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp) 131 | TARGET_LINK_LIBRARIES(test_pipeline svo) 132 | 133 | ADD_EXECUTABLE(test_pipel_euroc test/test_pipel_euroc.cpp) 134 | TARGET_LINK_LIBRARIES(test_pipel_euroc svo) 135 | 136 | ADD_EXECUTABLE(test_live_vo test/test_live_vo.cpp) 137 | TARGET_LINK_LIBRARIES(test_live_vo svo) 138 | -------------------------------------------------------------------------------- /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/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/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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SVO_edgelet 2 | ChangeList， compare with the origin svo: 3 | 1. add edgelete feature 4 | 2. inition VO: auto select H model and E model 5 | 3. keyframe selection strategy 6 | 4. add pangolin to draw trajectory and remove ROS 7 | 8 | ## install（ubuntu） 9 | 10 | ### Prerequisites 11 | * boost - c++ Librairies (thread and system are needed) 12 | > sudo apt-get install libboost-all-dev 13 | * Eigen 3 - Linear algebra 14 | > apt-get install libeigen3-dev 15 | * OpenCV3 16 | * Sophus - Lie groups 17 | we have intergrated sophus in our code. Thanks for the code: https://github.com/strasdat/Sophus.git 18 | 19 | * g2o - General Graph Optimization **OPTIONAL** 20 | Only required if you want to run bundle adjustment. It is not necessary for visual odometry. 21 | I suggest an out-of-source build of g2o: 22 | > cd workspace 23 | > 24 | > git clone https://github.com/RainerKuemmerle/g2o.git 25 | > 26 | > cd g2o 27 | > 28 | > mkdir build 29 | > 30 | > cd build 31 | > 32 | > cmake .. 33 | > 34 | > make 35 | > 36 | > sudo make install 37 | 38 | ### Build and Compile 39 | If you want use g2o, please set flag in cmakerlists.txt： 40 | >SET(HAVE_G2O TRUE) #TRUE FALSE 41 | 42 | compile： 43 | > mkdir build 44 | > cd build 45 | > cmake .. 46 | > make 47 | 48 | ## Run code 49 | test file, test_pipline and test_live_vo: 50 | 51 | Offline datasets： 52 | 53 | To run on tum dataset：test_pipline 54 | To run on euroc dataset：test_piplel_euroc 55 | 56 | Online camera：test_live_vo 57 | 58 | ## Results 59 | ![Results1](http://img.blog.csdn.net/20170312231721526?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvaGV5aWppYTAzMjc=/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast) 60 | ## Authors 61 | Yijia He, Institute of Automation, Chinese Academy of Sciences 62 | 63 | Wei Wei, Institute of Computing Technology, Chinese Academy of Sciences 64 | 65 | Yanan Gao, Institute of Computing Technology, Chinese Academy of Sciences 66 | -------------------------------------------------------------------------------- /bin/EuRoC.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 435.2046959714599 9 | Camera.fy: 435.2046959714599 10 | Camera.cx: 367.4517211914062 11 | Camera.cy: 252.2008514404297 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 752 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 20.0 23 | 24 | # stereo baseline times fx 25 | Camera.bf: 47.90639384423901 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 35 32 | 33 | #-------------------------------------------------------------------------------------------- 34 | # Stereo Rectification. Only if you need to pre-rectify the images. 35 | # Camera.fx, .fy, etc must be the same as in LEFT.P 36 | #-------------------------------------------------------------------------------------------- 37 | LEFT.height: 480 38 | LEFT.width: 752 39 | LEFT.D: !!opencv-matrix 40 | rows: 1 41 | cols: 5 42 | dt: d 43 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 44 | LEFT.K: !!opencv-matrix 45 | rows: 3 46 | cols: 3 47 | dt: d 48 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 49 | LEFT.R: !!opencv-matrix 50 | rows: 3 51 | cols: 3 52 | dt: d 53 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 54 | LEFT.P: !!opencv-matrix 55 | rows: 3 56 | cols: 4 57 | dt: d 58 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 59 | 60 | RIGHT.height: 480 61 | RIGHT.width: 752 62 | RIGHT.D: !!opencv-matrix 63 | rows: 1 64 | cols: 5 65 | dt: d 66 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 67 | RIGHT.K: !!opencv-matrix 68 | rows: 3 69 | cols: 3 70 | dt: d 71 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 72 | RIGHT.R: !!opencv-matrix 73 | rows: 3 74 | cols: 3 75 | dt: d 76 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 77 | RIGHT.P: !!opencv-matrix 78 | rows: 3 79 | cols: 4 80 | dt: d 81 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 82 | 83 | #-------------------------------------------------------------------------------------------- 84 | # ORB Parameters 85 | #-------------------------------------------------------------------------------------------- 86 | 87 | # ORB Extractor: Number of features per image 88 | ORBextractor.nFeatures: 1200 89 | 90 | # ORB Extractor: Scale factor between levels in the scale pyramid 91 | ORBextractor.scaleFactor: 1.2 92 | 93 | # ORB Extractor: Number of levels in the scale pyramid 94 | ORBextractor.nLevels: 8 95 | 96 | # ORB Extractor: Fast threshold 97 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 98 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 99 | # You can lower these values if your images have low contrast 100 | ORBextractor.iniThFAST: 20 101 | ORBextractor.minThFAST: 7 102 | 103 | #-------------------------------------------------------------------------------------------- 104 | # Viewer Parameters 105 | #-------------------------------------------------------------------------------------------- 106 | Viewer.KeyFrameSize: 0.05 107 | Viewer.KeyFrameLineWidth: 1 108 | Viewer.GraphLineWidth: 0.9 109 | Viewer.PointSize:2 110 | Viewer.CameraSize: 0.08 111 | Viewer.CameraLineWidth: 3 112 | Viewer.ViewpointX: 0 113 | Viewer.ViewpointY: -0.7 114 | Viewer.ViewpointZ: -1.8 115 | Viewer.ViewpointF: 500 116 | -------------------------------------------------------------------------------- /bin/test_live_vo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HeYijia/svo_edgelet/1f408d7c3c10d3a28184fd84d199d757e46af672/bin/test_live_vo -------------------------------------------------------------------------------- /bin/test_pipel_euroc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HeYijia/svo_edgelet/1f408d7c3c10d3a28184fd84d199d757e46af672/bin/test_pipel_euroc -------------------------------------------------------------------------------- /bin/test_pipeline: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HeYijia/svo_edgelet/1f408d7c3c10d3a28184fd84d199d757e46af672/bin/test_pipeline -------------------------------------------------------------------------------- /include/sophus/common.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Common functionality. 3 | 4 | #ifndef SOPHUS_COMMON_HPP 5 | #define SOPHUS_COMMON_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #if !defined(SOPHUS_DISABLE_ENSURES) 16 | #include "formatstring.hpp" 17 | #endif //!defined(SOPHUS_DISABLE_ENSURES) 18 | 19 | // following boost's assert.hpp 20 | #undef SOPHUS_ENSURE 21 | 22 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 23 | // release builds). At the moment there are no ASSERTS in Sophus which should 24 | // only be used for checks which are performance critical. 25 | 26 | #ifdef __GNUC__ 27 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 28 | #elif (_MSC_VER >= 1310) 29 | #define SOPHUS_FUNCTION __FUNCTION__ 30 | #else 31 | #define SOPHUS_FUNCTION "unknown" 32 | #endif 33 | 34 | // Make sure this compiles with older versions of Eigen which do not have 35 | // EIGEN_DEVICE_FUNC defined. 36 | #ifndef EIGEN_DEVICE_FUNC 37 | #define EIGEN_DEVICE_FUNC 38 | #endif 39 | 40 | // NVCC on windows has issues with defaulting the Map specialization constructors, so 41 | // special case that specific platform case. 42 | #if defined(_MSC_VER) && defined(__CUDACC__) 43 | #define SOPHUS_WINDOW_NVCC_FALLBACK 44 | #endif 45 | 46 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 47 | 48 | #if defined(SOPHUS_DISABLE_ENSURES) 49 | 50 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 51 | 52 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 53 | 54 | namespace Sophus { 55 | void ensureFailed(char const* function, char const* file, int line, 56 | char const* description); 57 | } 58 | 59 | #define SOPHUS_ENSURE(expr, ...) \ 60 | ((expr) ? ((void)0) \ 61 | : ::Sophus::ensureFailed( \ 62 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 63 | Sophus::details::FormatString(__VA_ARGS__).c_str())) 64 | #else 65 | // LCOV_EXCL_START 66 | 67 | namespace Sophus { 68 | template 69 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 70 | char const* description, Args&&... args) { 71 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 72 | function, file, line); 73 | #ifdef __CUDACC__ 74 | std::printf("%s", description); 75 | #else 76 | std::cout << details::FormatString(description, std::forward(args)...) 77 | << std::endl; 78 | std::abort(); 79 | #endif 80 | } 81 | } // namespace Sophus 82 | 83 | // LCOV_EXCL_STOP 84 | #define SOPHUS_ENSURE(expr, ...) \ 85 | ((expr) ? ((void)0) \ 86 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 87 | ##__VA_ARGS__)) 88 | #endif 89 | 90 | namespace Sophus { 91 | 92 | template 93 | struct Constants { 94 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 95 | 96 | SOPHUS_FUNC static Scalar epsilonSqrt() { 97 | using std::sqrt; 98 | return sqrt(epsilon()); 99 | } 100 | 101 | SOPHUS_FUNC static Scalar pi() { 102 | return Scalar(3.141592653589793238462643383279502884); 103 | } 104 | }; 105 | 106 | template <> 107 | struct Constants { 108 | SOPHUS_FUNC static float constexpr epsilon() { 109 | return static_cast(1e-5); 110 | } 111 | 112 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 113 | 114 | SOPHUS_FUNC static float constexpr pi() { 115 | return 3.141592653589793238462643383279502884f; 116 | } 117 | }; 118 | 119 | /// Nullopt type of lightweight optional class. 120 | struct nullopt_t { 121 | explicit constexpr nullopt_t() {} 122 | }; 123 | 124 | constexpr nullopt_t nullopt{}; 125 | 126 | /// Lightweight optional implementation which requires ``T`` to have a 127 | /// default constructor. 128 | /// 129 | /// TODO: Replace with std::optional once Sophus moves to c++17. 130 | /// 131 | template 132 | class optional { 133 | public: 134 | optional() : is_valid_(false) {} 135 | 136 | optional(nullopt_t) : is_valid_(false) {} 137 | 138 | optional(T const& type) : type_(type), is_valid_(true) {} 139 | 140 | explicit operator bool() const { return is_valid_; } 141 | 142 | T const* operator->() const { 143 | SOPHUS_ENSURE(is_valid_, "must be valid"); 144 | return &type_; 145 | } 146 | 147 | T* operator->() { 148 | SOPHUS_ENSURE(is_valid_, "must be valid"); 149 | return &type_; 150 | } 151 | 152 | T const& operator*() const { 153 | SOPHUS_ENSURE(is_valid_, "must be valid"); 154 | return type_; 155 | } 156 | 157 | T& operator*() { 158 | SOPHUS_ENSURE(is_valid_, "must be valid"); 159 | return type_; 160 | } 161 | 162 | private: 163 | T type_; 164 | bool is_valid_; 165 | }; 166 | 167 | template 168 | using enable_if_t = typename std::enable_if::type; 169 | 170 | template 171 | struct IsUniformRandomBitGenerator { 172 | static const bool value = std::is_unsigned::value && 173 | std::is_unsigned::value && 174 | std::is_unsigned::value; 175 | }; 176 | } // namespace Sophus 177 | 178 | #endif // SOPHUS_COMMON_HPP 179 | -------------------------------------------------------------------------------- /include/sophus/example_ensure_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "common.hpp" 2 | 3 | #include 4 | #include 5 | 6 | namespace Sophus { 7 | void ensureFailed(char const* function, char const* file, int line, 8 | char const* description) { 9 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 10 | file, function, line); 11 | std::printf("Description: %s\n", description); 12 | std::abort(); 13 | } 14 | } // namespace Sophus 15 | -------------------------------------------------------------------------------- /include/sophus/formatstring.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// FormatString functionality 3 | 4 | #ifndef SOPHUS_FORMATSTRING_HPP 5 | #define SOPHUS_FORMATSTRING_HPP 6 | 7 | #include 8 | 9 | namespace Sophus { 10 | namespace details { 11 | 12 | // Following: http://stackoverflow.com/a/22759544 13 | template 14 | class IsStreamable { 15 | private: 16 | template 17 | static auto test(int) 18 | -> decltype(std::declval() << std::declval(), 19 | std::true_type()); 20 | 21 | template 22 | static auto test(...) -> std::false_type; 23 | 24 | public: 25 | static bool const value = decltype(test(0))::value; 26 | }; 27 | 28 | template 29 | class ArgToStream { 30 | public: 31 | static void impl(std::stringstream& stream, T&& arg) { 32 | stream << std::forward(arg); 33 | } 34 | }; 35 | 36 | inline void FormatStream(std::stringstream& stream, char const* text) { 37 | stream << text; 38 | return; 39 | } 40 | 41 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 42 | template 43 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 44 | Args&&... args) { 45 | static_assert(IsStreamable::value, 46 | "One of the args has no ostream overload!"); 47 | for (; *text != '\0'; ++text) { 48 | if (*text == '%') { 49 | ArgToStream::impl(stream, std::forward(arg)); 50 | FormatStream(stream, text + 1, std::forward(args)...); 51 | return; 52 | } 53 | stream << *text; 54 | } 55 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 56 | << " args unused."; 57 | return; 58 | } 59 | 60 | template 61 | std::string FormatString(char const* text, Args&&... args) { 62 | std::stringstream stream; 63 | FormatStream(stream, text, std::forward(args)...); 64 | return stream.str(); 65 | } 66 | 67 | inline std::string FormatString() { return std::string(); } 68 | 69 | } // namespace details 70 | } // namespace Sophus 71 | 72 | #endif //SOPHUS_FORMATSTRING_HPP 73 | -------------------------------------------------------------------------------- /include/sophus/geometry.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Transformations between poses and hyperplanes. 3 | 4 | #ifndef GEOMETRY_HPP 5 | #define GEOMETRY_HPP 6 | 7 | #include "se2.hpp" 8 | #include "se3.hpp" 9 | #include "so2.hpp" 10 | #include "so3.hpp" 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding line 16 | /// normal along the y-axis (in reference frame ``foo``). 17 | /// 18 | template 19 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 20 | return R_foo_line.matrix().col(1); 21 | } 22 | 23 | /// Takes in line normal in reference frame foo and constructs a corresponding 24 | /// rotation matrix ``R_foo_line``. 25 | /// 26 | /// Precondition: ``normal_foo`` must not be close to zero. 27 | /// 28 | template 29 | SO2 SO2FromNormal(Vector2 normal_foo) { 30 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 31 | normal_foo.transpose()); 32 | normal_foo.normalize(); 33 | return SO2(normal_foo.y(), -normal_foo.x()); 34 | } 35 | 36 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 37 | /// normal along the z-axis 38 | /// (in reference frame ``foo``). 39 | /// 40 | template 41 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 42 | return R_foo_plane.matrix().col(2); 43 | } 44 | 45 | /// Takes in plane normal in reference frame foo and constructs a corresponding 46 | /// rotation matrix ``R_foo_plane``. 47 | /// 48 | /// Note: The ``plane`` frame is defined as such that the normal points along 49 | /// the positive z-axis. One can specify hints for the x-axis and y-axis 50 | /// of the ``plane`` frame. 51 | /// 52 | /// Preconditions: 53 | /// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 54 | /// zero. 55 | /// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 56 | /// 57 | template 58 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 59 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 60 | T(0)), 61 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 62 | T(0))) { 63 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 64 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 65 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 66 | using std::abs; 67 | using std::sqrt; 68 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 69 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 70 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 71 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 72 | xDirHint_foo.transpose()); 73 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 74 | yDirHint_foo.transpose()); 75 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 76 | normal_foo.transpose()); 77 | 78 | Matrix3 basis_foo; 79 | basis_foo.col(2) = normal_foo; 80 | 81 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | xDirHint_foo.normalize(); 83 | } 84 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | yDirHint_foo.normalize(); 86 | } 87 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 88 | basis_foo.col(2).normalize(); 89 | } 90 | 91 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 92 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 93 | if (abs_x_dot_z < abs_y_dot_z) { 94 | // basis_foo.z and xDirHint are far from parallel. 95 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 96 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 97 | } else { 98 | // basis_foo.z and yDirHint are far from parallel. 99 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 100 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 101 | } 102 | T det = basis_foo.determinant(); 103 | // sanity check 104 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 105 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 106 | basis_foo); 107 | return basis_foo; 108 | } 109 | 110 | /// Takes in plane normal in reference frame foo and constructs a corresponding 111 | /// rotation matrix ``R_foo_plane``. 112 | /// 113 | /// See ``rotationFromNormal`` for details. 114 | /// 115 | template 116 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 117 | return SO3(rotationFromNormal(normal_foo)); 118 | } 119 | 120 | /// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 121 | /// reference frame ``foo``. 122 | /// 123 | /// Note: The plane is defined by X-axis of the ``line`` frame. 124 | /// 125 | template 126 | Line2 lineFromSE2(SE2 const& T_foo_line) { 127 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 128 | } 129 | 130 | /// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 131 | /// 132 | /// Note: The line is defined by X-axis of the frame ``line``. 133 | /// 134 | template 135 | SE2 SE2FromLine(Line2 const& line_foo) { 136 | T const d = line_foo.offset(); 137 | Vector2 const n = line_foo.normal(); 138 | SO2 const R_foo_plane = SO2FromNormal(n); 139 | return SE2(R_foo_plane, -d * n); 140 | } 141 | 142 | /// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 143 | /// reference frame ``foo``. 144 | /// 145 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 146 | /// 147 | template 148 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 149 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 150 | } 151 | 152 | /// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 153 | /// 154 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 155 | /// 156 | template 157 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 158 | T const d = plane_foo.offset(); 159 | Vector3 const n = plane_foo.normal(); 160 | SO3 const R_foo_plane = SO3FromNormal(n); 161 | return SE3(R_foo_plane, -d * n); 162 | } 163 | 164 | /// Takes in a hyperplane and returns unique representation by ensuring that the 165 | /// ``offset`` is not negative. 166 | /// 167 | template 168 | Eigen::Hyperplane makeHyperplaneUnique( 169 | Eigen::Hyperplane const& plane) { 170 | if (plane.offset() >= 0) { 171 | return plane; 172 | } 173 | 174 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 175 | } 176 | 177 | } // namespace Sophus 178 | 179 | #endif // GEOMETRY_HPP 180 | -------------------------------------------------------------------------------- /include/sophus/interpolate.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Interpolation for Lie groups. 3 | 4 | #ifndef SOPHUS_INTERPOLATE_HPP 5 | #define SOPHUS_INTERPOLATE_HPP 6 | 7 | #include 8 | 9 | #include "interpolate_details.hpp" 10 | 11 | namespace Sophus { 12 | 13 | /// This function interpolates between two Lie group elements ``foo_T_bar`` 14 | /// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 15 | /// 16 | /// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 17 | /// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 18 | /// ``foo_T_bar``. 19 | /// 20 | /// (Since interpolation on Lie groups is inverse-invariant, we can equivalently 21 | /// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 22 | /// return value being ``quiz_T_foo``.) 23 | /// 24 | /// Precondition: ``p`` must be in [0, 1]. 25 | /// 26 | template 27 | enable_if_t::supported, G> interpolate( 28 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 29 | using Scalar = typename G::Scalar; 30 | Scalar inter_p(p); 31 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 32 | "p (%) must in [0, 1]."); 33 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 34 | } 35 | 36 | } // namespace Sophus 37 | 38 | #endif // SOPHUS_INTERPOLATE_HPP 39 | -------------------------------------------------------------------------------- /include/sophus/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace interp_details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /include/sophus/num_diff.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Numerical differentiation using finite differences 3 | 4 | #ifndef SOPHUS_NUM_DIFF_HPP 5 | #define SOPHUS_NUM_DIFF_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | namespace details { 16 | template 17 | class Curve { 18 | public: 19 | template 20 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 21 | using ReturnType = decltype(curve(t)); 22 | static_assert(std::is_floating_point::value, 23 | "Scalar must be a floating point type."); 24 | static_assert(IsFloatingPoint::value, 25 | "ReturnType must be either a floating point scalar, " 26 | "vector or matrix."); 27 | 28 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 29 | } 30 | }; 31 | 32 | template 33 | class VectorField { 34 | public: 35 | static Eigen::Matrix num_diff( 36 | std::function(Sophus::Vector)> 37 | vector_field, 38 | Sophus::Vector const& a, Scalar eps) { 39 | static_assert(std::is_floating_point::value, 40 | "Scalar must be a floating point type."); 41 | Eigen::Matrix J; 42 | Sophus::Vector h; 43 | h.setZero(); 44 | for (int i = 0; i < M; ++i) { 45 | h[i] = eps; 46 | J.col(i) = 47 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 48 | h[i] = Scalar(0); 49 | } 50 | 51 | return J; 52 | } 53 | }; 54 | 55 | template 56 | class VectorField { 57 | public: 58 | static Eigen::Matrix num_diff( 59 | std::function(Scalar)> vector_field, 60 | Scalar const& a, Scalar eps) { 61 | return details::Curve::num_diff(std::move(vector_field), a, eps); 62 | } 63 | }; 64 | } // namespace details 65 | 66 | /// Calculates the derivative of a curve at a point ``t``. 67 | /// 68 | /// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 69 | /// returns either a Scalar, a vector or a matrix. 70 | /// 71 | template 72 | auto curveNumDiff(Fn curve, Scalar t, 73 | Scalar h = Constants::epsilonSqrt()) 74 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 75 | return details::Curve::num_diff(std::move(curve), t, h); 76 | } 77 | 78 | /// Calculates the derivative of a vector field at a point ``a``. 79 | /// 80 | /// Here, a vector field is a function from a vector space to another vector 81 | /// space. 82 | /// 83 | template 84 | Eigen::Matrix vectorFieldNumDiff( 85 | Fn vector_field, ScalarOrVector const& a, 86 | Scalar eps = Constants::epsilonSqrt()) { 87 | return details::VectorField::num_diff(std::move(vector_field), 88 | a, eps); 89 | } 90 | 91 | } // namespace Sophus 92 | 93 | #endif // SOPHUS_NUM_DIFF_HPP 94 | -------------------------------------------------------------------------------- /include/sophus/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Rotation matrix helper functions. 3 | 4 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 5 | #define SOPHUS_ROTATION_MATRIX_HPP 6 | 7 | #include 8 | #include 9 | 10 | #include "types.hpp" 11 | 12 | namespace Sophus { 13 | 14 | /// Takes in arbitrary square matrix and returns true if it is 15 | /// orthogonal. 16 | template 17 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 18 | using Scalar = typename D::Scalar; 19 | static int const N = D::RowsAtCompileTime; 20 | static int const M = D::ColsAtCompileTime; 21 | 22 | static_assert(N == M, "must be a square matrix"); 23 | static_assert(N >= 2, "must have compile time dimension >= 2"); 24 | 25 | return (R * R.transpose() - Matrix::Identity()).norm() < 26 | Constants::epsilon(); 27 | } 28 | 29 | /// Takes in arbitrary square matrix and returns true if it is 30 | /// "scaled-orthogonal" with positive determinant. 31 | /// 32 | template 33 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 34 | using Scalar = typename D::Scalar; 35 | static int const N = D::RowsAtCompileTime; 36 | static int const M = D::ColsAtCompileTime; 37 | using std::pow; 38 | using std::sqrt; 39 | 40 | Scalar det = sR.determinant(); 41 | 42 | if (det <= Scalar(0)) { 43 | return false; 44 | } 45 | 46 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 47 | 48 | static_assert(N == M, "must be a square matrix"); 49 | static_assert(N >= 2, "must have compile time dimension >= 2"); 50 | 51 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 52 | .template lpNorm() < 53 | sqrt(Constants::epsilon()); 54 | } 55 | 56 | /// Takes in arbitrary square matrix (2x2 or larger) and returns closest 57 | /// orthogonal matrix with positive determinant. 58 | template 59 | SOPHUS_FUNC enable_if_t< 60 | std::is_floating_point::value, 61 | Matrix> 62 | makeRotationMatrix(Eigen::MatrixBase const& R) { 63 | using Scalar = typename D::Scalar; 64 | static int const N = D::RowsAtCompileTime; 65 | static int const M = D::ColsAtCompileTime; 66 | 67 | static_assert(N == M, "must be a square matrix"); 68 | static_assert(N >= 2, "must have compile time dimension >= 2"); 69 | 70 | Eigen::JacobiSVD> svd( 71 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 72 | 73 | // Determine determinant of orthogonal matrix U*V'. 74 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 75 | // Starting from the identity matrix D, set the last entry to d (+1 or 76 | // -1), so that det(U*D*V') = 1. 77 | Matrix Diag = Matrix::Identity(); 78 | Diag(N - 1, N - 1) = d; 79 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 80 | } 81 | 82 | } // namespace Sophus 83 | 84 | #endif // SOPHUS_ROTATION_MATRIX_HPP 85 | -------------------------------------------------------------------------------- /include/sophus/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::cos; 14 | using std::exp; 15 | using std::sin; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::cos; 57 | using std::sin; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // namespace details 101 | } // namespace Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /include/sophus/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | -------------------------------------------------------------------------------- /include/svo/blender_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * blender_utils.h 3 | * 4 | * Created on: Feb 13, 2014 5 | * Author: cforster 6 | */ 7 | 8 | #ifndef BLENDER_UTILS_H_ 9 | #define BLENDER_UTILS_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace svo { 19 | 20 | /** 21 | * Entry has to support the following operator 22 | * std::istream& operator >>(std::istream&, Entry&); 23 | */ 24 | template 25 | class FileReader 26 | { 27 | public: 28 | FileReader(const std::string& file) : 29 | hasEntry_(false), 30 | file_(file), 31 | file_stream_(file.c_str()) 32 | {} 33 | 34 | virtual ~FileReader() 35 | { 36 | file_stream_.close(); 37 | } 38 | 39 | void skip(int num_lines) 40 | { 41 | for(int idx = 0; idx < num_lines; ++idx) 42 | { 43 | if(!file_stream_.good()) continue; 44 | file_stream_.ignore(1024, '\n'); 45 | assert(file_stream_.gcount() < 1024); 46 | } 47 | } 48 | 49 | void skipComments() 50 | { 51 | while(file_stream_.good() && file_stream_.peek() == '#') 52 | skip(1); 53 | } 54 | 55 | /// Moves to the next entry in the file. Returns true, if there was a next entry, false otherwise. 56 | bool next() 57 | { 58 | if(file_stream_.good() && !file_stream_.eof()) 59 | { 60 | file_stream_ >> entry_; 61 | hasEntry_ = true; 62 | return true; 63 | } 64 | return false; 65 | } 66 | 67 | /// Read all entries at once. 68 | void readAllEntries(std::vector& entries) 69 | { 70 | if(!hasEntry()) next(); 71 | do 72 | entries.push_back(entry()); 73 | while(next()); 74 | } 75 | 76 | /// Gets the current entry 77 | const Entry& entry() const { return entry_; } 78 | 79 | /// Determines whether the first entry was read 80 | const bool& hasEntry() const { return hasEntry_; } 81 | 82 | private: 83 | bool hasEntry_; 84 | std::string file_; 85 | std::ifstream file_stream_; 86 | Entry entry_; 87 | }; 88 | 89 | namespace blender_utils { 90 | 91 | void loadBlenderDepthmap( 92 | const std::string file_name, 93 | const svo::AbstractCamera& cam, 94 | cv::Mat& img) 95 | { 96 | std::ifstream file_stream(file_name.c_str()); 97 | assert(file_stream.is_open()); 98 | img = cv::Mat(cam.height(), cam.width(), CV_32FC1); 99 | float * img_ptr = img.ptr(); 100 | float depth; 101 | for(int y=0; y> depth; 106 | // blender: 107 | Eigen::Vector3d xyz ( cam.cam2world(x,y)) ; 108 | Eigen::Vector2d uv = xyz.head<2>()/xyz[2];; 109 | *img_ptr = depth * sqrt(uv[0]*uv[0] + uv[1]*uv[1] + 1.0); 110 | 111 | // povray 112 | // *img_ptr = depth/100.0; // depth is in [cm], we want [m] 113 | 114 | if(file_stream.peek() == '\n' && x != cam.width()-1 && y != cam.height()-1) 115 | printf("WARNING: did not read the full depthmap!\n"); 116 | } 117 | } 118 | } 119 | 120 | bool getDepthmapNormalAtPoint( 121 | const Vector2i& px, 122 | const cv::Mat& depth, 123 | const int halfpatch_size, 124 | const svo::AbstractCamera& cam, 125 | Vector3d& normal) 126 | { 127 | assert(cam.width() == depth.cols && cam.height() == depth.rows); 128 | if(!cam.isInFrame(px, halfpatch_size+1)) 129 | return false; 130 | 131 | const size_t n_meas = (halfpatch_size*2+1)*(halfpatch_size*2+1); 132 | list pts; 133 | for(int y = px[1]-halfpatch_size; y<=px[1]+halfpatch_size; ++y) 134 | for(int x = px[0]-halfpatch_size; x<=px[0]+halfpatch_size; ++x) 135 | pts.push_back(cam.cam2world(x,y)*depth.at(y,x)); 136 | 137 | assert(n_meas == pts.size()); 138 | Eigen::Matrix A; A.resize(n_meas, Eigen::NoChange); 139 | Eigen::Matrix b; b.resize(n_meas, Eigen::NoChange); 140 | 141 | size_t i = 0; 142 | for(list::iterator it=pts.begin(); it!=pts.end(); ++it) 143 | { 144 | A.row(i) << it->x(), it->y(), it->z(), 1.0; 145 | b[i] = 0; 146 | ++i; 147 | } 148 | 149 | JacobiSVD svd(A, ComputeThinU | ComputeThinV); 150 | 151 | Eigen::Matrix V = svd.matrixV(); 152 | normal = V.block<3,1>(0,3); 153 | normal.normalize(); 154 | return true; 155 | } 156 | 157 | namespace file_format 158 | { 159 | 160 | class ImageNameAndPose 161 | { 162 | public: 163 | ImageNameAndPose() {} 164 | virtual ~ImageNameAndPose() {} 165 | double timestamp_; 166 | std::string image_name_; 167 | Eigen::Vector3d t_; 168 | Eigen::Quaterniond q_; 169 | friend std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& pair); 170 | friend std::istream& operator >>(std::istream& in, ImageNameAndPose& pair); 171 | }; 172 | 173 | std::ostream& operator <<(std::ostream& out, const ImageNameAndPose& gt) 174 | { 175 | out << gt.timestamp_ << " " << gt.image_name_ << " " 176 | << gt.t_.x() << " " << gt.t_.y() << " " << gt.t_.z() << " " 177 | << gt.q_.x() << " " << gt.q_.y() << " " << gt.q_.z() << " " << gt.q_.w() << " " << std::endl; 178 | return out; 179 | } 180 | 181 | std::istream& operator >>(std::istream& in, ImageNameAndPose& gt) 182 | { 183 | in >> gt.timestamp_; 184 | in >> gt.image_name_; 185 | double tx, ty, tz, qx, qy, qz, qw; 186 | in >> tx; 187 | in >> ty; 188 | in >> tz; 189 | in >> qx; 190 | in >> qy; 191 | in >> qz; 192 | in >> qw; 193 | gt.t_ = Eigen::Vector3d(tx, ty, tz); 194 | gt.q_ = Eigen::Quaterniond(qw, qx, qy, qz); 195 | gt.q_.normalize(); 196 | return in; 197 | } 198 | 199 | } // namespace file_format 200 | } // namespace blender_utils 201 | } // namespace vk 202 | 203 | #endif // VIKIT_BLENDER_UTILS_H_ 204 | -------------------------------------------------------------------------------- /include/svo/bundle_adjustment.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_BUNDLE_ADJUSTMENT_H_ 18 | #define SVO_BUNDLE_ADJUSTMENT_H_ 19 | 20 | #include 21 | 22 | namespace g2o { 23 | class EdgeProjectXYZ2UV; 24 | class SparseOptimizer; 25 | class VertexSE3Expmap; 26 | class VertexSBAPointXYZ; 27 | } 28 | 29 | namespace svo { 30 | 31 | typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3; 32 | typedef g2o::VertexSE3Expmap g2oFrameSE3; 33 | typedef g2o::VertexSBAPointXYZ g2oPoint; 34 | 35 | class Frame; 36 | class Point; 37 | class Feature; 38 | class Map; 39 | 40 | /// Local, global and 2-view bundle adjustment with g2o 41 | namespace ba { 42 | 43 | /// Temporary container to hold the g2o edge with reference to frame and point. 44 | struct EdgeContainerSE3 45 | { 46 | g2oEdgeSE3* edge; 47 | Frame* frame; 48 | Feature* feature; 49 | bool is_deleted; 50 | EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) : 51 | edge(e), frame(frame), feature(feature), is_deleted(false) 52 | {} 53 | }; 54 | 55 | /// Optimize two camera frames and their observed 3D points. 56 | /// Is used after initialization. 57 | void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map); 58 | 59 | /// Local bundle adjustment. 60 | /// Optimizes core_kfs and their observed map points while keeping the 61 | /// neighbourhood fixed. 62 | void localBA( 63 | Frame* center_kf, 64 | set* core_kfs, 65 | Map* map, 66 | size_t& n_incorrect_edges_1, 67 | size_t& n_incorrect_edges_2, 68 | double& init_error, 69 | double& final_error); 70 | 71 | /// Global bundle adjustment. 72 | /// Optimizes the whole map. Is currently not used in SVO. 73 | void globalBA(Map* map); 74 | 75 | /// Initialize g2o with solver type, optimization strategy and camera model. 76 | void setupG2o(g2o::SparseOptimizer * optimizer); 77 | 78 | /// Run the optimization on the provided graph. 79 | void runSparseBAOptimizer( 80 | g2o::SparseOptimizer* optimizer, 81 | unsigned int num_iter, 82 | double& init_error, 83 | double& final_error); 84 | 85 | /// Create a g2o vertice from a keyframe object. 86 | g2oFrameSE3* createG2oFrameSE3( 87 | Frame* kf, 88 | size_t id, 89 | bool fixed); 90 | 91 | /// Creates a g2o vertice from a mappoint object. 92 | g2oPoint* createG2oPoint( 93 | Vector3d pos, 94 | size_t id, 95 | bool fixed); 96 | 97 | /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. 98 | g2oEdgeSE3* createG2oEdgeSE3( 99 | g2oFrameSE3* v_kf, 100 | g2oPoint* v_mp, 101 | const Vector2d& f_up, 102 | bool robust_kernel, 103 | double huber_width, 104 | double weight = 1); 105 | 106 | } // namespace ba 107 | } // namespace svo 108 | 109 | #endif // SVO_BUNDLE_ADJUSTMENT_H_ 110 | -------------------------------------------------------------------------------- /include/svo/camera_model.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_MODEL_H_ 2 | #define CAMERA_MODEL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace svo { 10 | 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | class AbstractCamera 15 | { 16 | protected: 17 | 18 | int width_; // TODO cannot be const because of omni-camera model 19 | int height_; 20 | 21 | public: 22 | 23 | AbstractCamera() {}; // need this constructor for omni camera 24 | AbstractCamera(int width, int height) : width_(width), height_(height) {}; 25 | 26 | virtual ~AbstractCamera() {}; 27 | 28 | /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. 29 | virtual Vector3d 30 | cam2world(const double& x, const double& y) const = 0; 31 | 32 | /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. 33 | virtual Vector3d 34 | cam2world(const Vector2d& px) const = 0; 35 | 36 | virtual Vector2d 37 | world2cam(const Vector3d& xyz_c) const = 0; 38 | 39 | /// projects unit plane coordinates to camera coordinates 40 | virtual Vector2d 41 | world2cam(const Vector2d& uv) const = 0; 42 | 43 | // undistort a point 44 | virtual Vector2d 45 | undistortpoint(const double& u, const double& v) const=0; // hyj 46 | 47 | virtual double 48 | errorMultiplier2() const = 0; 49 | 50 | virtual double 51 | errorMultiplier() const = 0; 52 | 53 | inline int width() const { return width_; } 54 | 55 | inline int height() const { return height_; } 56 | 57 | inline bool isInFrame(const Vector2i & obs, int boundary=0) const 58 | { 59 | if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_CONFIG_H_ 18 | #define SVO_CONFIG_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace svo { 25 | 26 | using std::string; 27 | 28 | /// Global configuration file of SVO. 29 | /// Implements the Singleton design pattern to allow global access and to ensure 30 | /// that only one instance exists. 31 | class Config 32 | { 33 | public: 34 | static Config& getInstance(); 35 | 36 | /// Base-name of the tracefiles. 37 | static string& traceName() { return getInstance().trace_name; } 38 | 39 | /// Directory where the tracefiles are saved. 40 | static string& traceDir() { return getInstance().trace_dir; } 41 | 42 | /// Number of pyramid levels used for features. 43 | static size_t& nPyrLevels() { return getInstance().n_pyr_levels; } 44 | 45 | /// Use the IMU to get relative rotations. 46 | static bool& useImu() { return getInstance().use_imu; } 47 | 48 | /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment. 49 | static size_t& coreNKfs() { return getInstance().core_n_kfs; } 50 | 51 | /// Initial scale of the map. Depends on the distance the camera is moved for the initialization. 52 | static double& mapScale() { return getInstance().map_scale; } 53 | 54 | /// Feature grid size of a cell in [px]. 55 | static size_t& gridSize() { return getInstance().grid_size; } 56 | 57 | /// Initialization: Minimum required disparity between the first two frames. 58 | static double& initMinDisparity() { return getInstance().init_min_disparity; } 59 | 60 | /// Initialization: Minimum number of tracked features. 61 | static size_t& initMinTracked() { return getInstance().init_min_tracked; } 62 | 63 | /// Initialization: Minimum number of inliers after RANSAC. 64 | static size_t& initMinInliers() { return getInstance().init_min_inliers; } 65 | 66 | /// Maximum level of the Lucas Kanade tracker. 67 | static size_t& kltMaxLevel() { return getInstance().klt_max_level; } 68 | 69 | /// Minimum level of the Lucas Kanade tracker. 70 | static size_t& kltMinLevel() { return getInstance().klt_min_level; } 71 | 72 | /// Reprojection threshold [px]. 73 | static double& reprojThresh() { return getInstance().reproj_thresh; } 74 | 75 | /// Reprojection threshold after pose optimization. 76 | static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } 77 | 78 | /// Number of iterations in local bundle adjustment. 79 | static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } 80 | 81 | /// Maximum number of points to optimize at every iteration. 82 | static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; } 83 | 84 | /// Number of iterations in structure optimization. 85 | static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; } 86 | 87 | /// Reprojection threshold after bundle adjustment. 88 | static double& lobaThresh() { return getInstance().loba_thresh; } 89 | 90 | /// Threshold for the robust Huber kernel of the local bundle adjustment. 91 | static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } 92 | 93 | /// Number of iterations in the local bundle adjustment. 94 | static size_t& lobaNumIter() { return getInstance().loba_num_iter; } 95 | 96 | /// Minimum distance between two keyframes. Relative to the average height in the map. 97 | static double& kfSelectMinDist() { return getInstance().kfselect_mindist; } 98 | 99 | /// Select only features with a minimum Harris corner score for triangulation. 100 | static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; } 101 | 102 | /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required! 103 | static size_t& subpixNIter() { return getInstance().subpix_n_iter; } 104 | 105 | /// Limit the number of keyframes in the map. This makes nslam essentially. 106 | /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. 107 | /// Minimum number of keyframes is 3. 108 | static size_t& maxNKfs() { return getInstance().max_n_kfs; } 109 | 110 | /// How much (in milliseconds) is the camera delayed with respect to the imu. 111 | static double& imgImuDelay() { return getInstance().img_imu_delay; } 112 | 113 | /// Maximum number of features that should be tracked. 114 | static size_t& maxFts() { return getInstance().max_fts; } 115 | 116 | /// If the number of tracked features drops below this threshold. Tracking quality is bad. 117 | static size_t& qualityMinFts() { return getInstance().quality_min_fts; } 118 | 119 | /// If within one frame, this amount of features are dropped. Tracking quality is bad. 120 | static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } 121 | 122 | private: 123 | Config(); 124 | Config(Config const&); 125 | void operator=(Config const&); 126 | string trace_name; 127 | string trace_dir; 128 | size_t n_pyr_levels; 129 | bool use_imu; 130 | size_t core_n_kfs; 131 | double map_scale; 132 | size_t grid_size; 133 | double init_min_disparity; 134 | size_t init_min_tracked; 135 | size_t init_min_inliers; 136 | size_t klt_max_level; 137 | size_t klt_min_level; 138 | double reproj_thresh; 139 | double poseoptim_thresh; 140 | size_t poseoptim_num_iter; 141 | size_t structureoptim_max_pts; 142 | size_t structureoptim_num_iter; 143 | double loba_thresh; 144 | double loba_robust_huber_width; 145 | size_t loba_num_iter; 146 | double kfselect_mindist; 147 | double triang_min_corner_score; 148 | size_t triang_half_patch_size; 149 | size_t subpix_n_iter; 150 | size_t max_n_kfs; 151 | double img_imu_delay; 152 | size_t max_fts; 153 | size_t quality_min_fts; 154 | int quality_max_drop_fts; 155 | }; 156 | 157 | } // namespace svo 158 | 159 | #endif // SVO_CONFIG_H_ 160 | -------------------------------------------------------------------------------- /include/svo/debug.h: -------------------------------------------------------------------------------- 1 | #ifndef DEBUG_H 2 | #define DEBUG_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace svo 12 | { 13 | 14 | class Timer 15 | { 16 | private: 17 | timeval start_time_; 18 | double time_; 19 | double accumulated_; 20 | public: 21 | 22 | /// The constructor directly starts the timer. 23 | Timer() : 24 | time_(0.0), 25 | accumulated_(0.0) 26 | { 27 | start(); 28 | } 29 | 30 | ~Timer() 31 | {} 32 | 33 | inline void start() 34 | { 35 | accumulated_ = 0.0; 36 | gettimeofday(&start_time_, NULL); 37 | } 38 | 39 | inline void resume() 40 | { 41 | gettimeofday(&start_time_, NULL); 42 | } 43 | 44 | inline double stop() 45 | { 46 | timeval end_time; 47 | gettimeofday(&end_time, NULL); 48 | long seconds = end_time.tv_sec - start_time_.tv_sec; 49 | long useconds = end_time.tv_usec - start_time_.tv_usec; 50 | time_ = ((seconds) + useconds*0.000001) + accumulated_; 51 | accumulated_ = time_; 52 | return time_; 53 | } 54 | 55 | inline double getTime() const 56 | { 57 | return time_; 58 | } 59 | 60 | inline void reset() 61 | { 62 | time_ = 0.0; 63 | accumulated_ = 0.0; 64 | } 65 | 66 | static double getCurrentTime() 67 | { 68 | timeval time_now; 69 | gettimeofday(&time_now, NULL); 70 | return time_now.tv_sec + time_now.tv_usec*0.000001; 71 | } 72 | 73 | static double getCurrentSecond() 74 | { 75 | timeval time_now; 76 | gettimeofday(&time_now, NULL); 77 | return time_now.tv_sec; 78 | } 79 | 80 | }; 81 | 82 | struct LogItem 83 | { 84 | double data; 85 | bool set; 86 | }; 87 | 88 | class DeBuger 89 | { 90 | public: 91 | DeBuger(); 92 | ~DeBuger(); 93 | void init(const std::string& trace_name, const std::string& trace_dir); 94 | void addTimer(const std::string& name); 95 | void addLog(const std::string& name); 96 | void writeToFile(); 97 | void startTimer(const std::string& name); 98 | void stopTimer(const std::string& name); 99 | double getTime(const std::string& name) const; 100 | void log(const std::string& name, double data); 101 | 102 | private: 103 | std::map timers_; 104 | std::map logs_; 105 | std::string trace_name_; // 5 | #include 6 | 7 | namespace fast 8 | { 9 | 10 | using ::std::vector; 11 | 12 | struct fast_xy 13 | { 14 | short x, y; 15 | fast_xy(short x_, short y_) : x(x_), y(y_) {} 16 | }; 17 | 18 | typedef unsigned char fast_byte; 19 | 20 | /// SSE2 optimized version of the corner 10 21 | void fast_corner_detect_10_sse2(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 22 | 23 | /// plain C++ version of the corner 10 24 | void fast_corner_detect_10(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 25 | 26 | /// corner score 10 27 | void fast_corner_score_10(const fast_byte* img, const int img_stride, const vector& corners, const int threshold, vector& scores); 28 | 29 | /// Nonmax Suppression on a 3x3 Window 30 | void fast_nonmax_3x3(const vector& corners, const vector& scores, vector& nonmax_corners); 31 | 32 | /// NEON optimized version of the corner 9 33 | void fast_corner_detect_9_neon(const fast_byte* img, int imgWidth, int imgHeight, int widthStep, short barrier, vector& corners); 34 | 35 | float shiTomasiScore(const cv::Mat& img, int u, int v); 36 | 37 | } // namespace fast 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/svo/faster_corner_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef FAST_CORNER_UTILITIES_H 2 | #define FAST_CORNER_UTILITIES_H 3 | 4 | #if __ARM_NEON__ 5 | #include 6 | #elif __SSE2__ 7 | #include 8 | #endif 9 | 10 | namespace fast 11 | { 12 | 13 | /// Check if the pointer is aligned to the specified byte granularity 14 | template bool is_aligned(const void* ptr); 15 | template<> inline bool is_aligned<8>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0x7) == 0; } 16 | template<> inline bool is_aligned<16>(const void* ptr) { return ((reinterpret_cast(ptr)) & 0xF) == 0; } 17 | 18 | 19 | struct Less 20 | { 21 | template static bool eval(const T1 a, const T2 b) 22 | { 23 | return a < b; 24 | } 25 | static short prep_t(short pixel_val, short barrier) 26 | { 27 | return pixel_val - barrier; 28 | } 29 | }; 30 | 31 | struct Greater 32 | { 33 | template static bool eval(const T1 a, const T2 b) 34 | { 35 | return a > b; 36 | } 37 | static short prep_t(short pixel_val, short barrier) 38 | { 39 | return pixel_val + barrier; 40 | } 41 | }; 42 | 43 | #if __SSE2__ 44 | 45 | #define CHECK_BARRIER(lo, hi, other, flags) \ 46 | { \ 47 | __m128i diff = _mm_subs_epu8(lo, other); \ 48 | __m128i diff2 = _mm_subs_epu8(other, hi); \ 49 | __m128i z = _mm_setzero_si128(); \ 50 | diff = _mm_cmpeq_epi8(diff, z); \ 51 | diff2 = _mm_cmpeq_epi8(diff2, z); \ 52 | flags = ~(_mm_movemask_epi8(diff) | (_mm_movemask_epi8(diff2) << 16)); \ 53 | } 54 | 55 | template inline __m128i load_si128(const void* addr) { return _mm_loadu_si128((const __m128i*)addr); } 56 | template <> inline __m128i load_si128(const void* addr) { return _mm_load_si128((const __m128i*)addr); } 57 | 58 | #endif 59 | 60 | } // namespace fast 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /include/svo/feature.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FEATURE_H_ 18 | #define SVO_FEATURE_H_ 19 | 20 | #include 21 | 22 | namespace svo { 23 | 24 | /// A salient image region that is tracked across frames. 25 | struct Feature 26 | { 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | enum FeatureType { 30 | CORNER, 31 | EDGELET 32 | }; 33 | 34 | FeatureType type; //!< Type can be corner or edgelet. 35 | Frame* frame; //!< Pointer to frame in which the feature was detected. 36 | Vector2d px; //!< Coordinates in pixels on pyramid level 0. 37 | Vector3d f; //!< Unit-bearing vector of the feature. 38 | int level; //!< Image pyramid level where feature was extracted. 39 | Point* point; //!< Pointer to 3D point which corresponds to the feature. 40 | Vector2d grad; //!< Dominant gradient direction for edglets, normalized. 41 | Vector2d grad_cur_ ; // edgelete grad direction in cur frame hyj 42 | 43 | Feature(Frame *_frame, const Vector2d &_px, const Vector2d &_grad, int _level): 44 | type(EDGELET), 45 | frame(_frame), 46 | px(_px), 47 | f(frame->cam_->cam2world(px)), 48 | level(_level), 49 | point(NULL), 50 | grad(_grad) 51 | {} 52 | 53 | Feature(Frame* _frame, const Vector2d& _px, int _level) : 54 | type(CORNER), 55 | frame(_frame), 56 | px(_px), 57 | f(frame->cam_->cam2world(px)), 58 | level(_level), 59 | point(NULL), 60 | grad(1.0,0.0) 61 | {} 62 | 63 | Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) : 64 | type(CORNER), 65 | frame(_frame), 66 | px(_px), 67 | f(_f), 68 | level(_level), 69 | point(NULL), 70 | grad(1.0,0.0) 71 | {} 72 | 73 | Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) : 74 | type(CORNER), 75 | frame(_frame), 76 | px(_px), 77 | f(_f), 78 | level(_level), 79 | point(_point), 80 | grad(1.0,0.0) 81 | {} 82 | 83 | Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, const Vector2d &_grad,int _level) : 84 | type(EDGELET), 85 | frame(_frame), 86 | px(_px), 87 | f(_f), 88 | level(_level), 89 | point(_point), 90 | grad(_grad) 91 | {} 92 | 93 | }; 94 | 95 | } // namespace svo 96 | 97 | #endif // SVO_FEATURE_H_ 98 | -------------------------------------------------------------------------------- /include/svo/feature_alignment.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FEATURE_ALIGNMENT_H_ 18 | #define SVO_FEATURE_ALIGNMENT_H_ 19 | 20 | #include 21 | 22 | namespace svo { 23 | 24 | /// Subpixel refinement of a reference feature patch with the current image. 25 | /// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" 26 | /// paper by Baker. 27 | namespace feature_alignment { 28 | 29 | bool align1D( 30 | const cv::Mat& cur_img, 31 | const Vector2f& dir, // direction in which the patch is allowed to move 32 | uint8_t* ref_patch_with_border, 33 | uint8_t* ref_patch, 34 | const int n_iter, 35 | Vector2d& cur_px_estimate, 36 | double& h_inv); 37 | 38 | bool align2D( 39 | const cv::Mat& cur_img, 40 | uint8_t* ref_patch_with_border, 41 | uint8_t* ref_patch, 42 | const int n_iter, 43 | Vector2d& cur_px_estimate, 44 | bool no_simd = false); 45 | 46 | bool align2D_SSE2( 47 | const cv::Mat& cur_img, 48 | uint8_t* ref_patch_with_border, 49 | uint8_t* ref_patch, 50 | const int n_iter, 51 | Vector2d& cur_px_estimate); 52 | 53 | bool align2D_NEON( 54 | const cv::Mat& cur_img, 55 | uint8_t* ref_patch_with_border, 56 | uint8_t* ref_patch, 57 | const int n_iter, 58 | Vector2d& cur_px_estimate); 59 | 60 | } // namespace feature_alignment 61 | } // namespace svo 62 | 63 | #endif // SVO_FEATURE_ALIGNMENT_H_ 64 | -------------------------------------------------------------------------------- /include/svo/feature_detection.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FEATURE_DETECTION_H_ 18 | #define SVO_FEATURE_DETECTION_H_ 19 | 20 | #include 21 | #include 22 | 23 | namespace svo { 24 | 25 | /// Implementation of various feature detectors. 26 | namespace feature_detection { 27 | 28 | /// Temporary container used for corner detection. Features are initialized from these. 29 | struct Corner 30 | { 31 | int x; //!< x-coordinate of corner in the image. 32 | int y; //!< y-coordinate of corner in the image. 33 | int level; //!< pyramid level of the corner. 34 | float score; //!< shi-tomasi score of the corner. 35 | float angle; //!< for gradient-features: dominant gradient angle. 36 | Corner(int x, int y, float score, int level, float angle) : 37 | x(x), y(y), level(level), score(score), angle(angle) 38 | {} 39 | }; 40 | typedef vector Corners; 41 | 42 | /// All detectors should derive from this abstract class. 43 | class AbstractDetector 44 | { 45 | public: 46 | AbstractDetector( 47 | const int img_width, 48 | const int img_height, 49 | const int cell_size, 50 | const int n_pyr_levels); 51 | 52 | virtual ~AbstractDetector() {}; 53 | 54 | virtual void detect( 55 | Frame* frame, 56 | const ImgPyr& img_pyr, 57 | const double detection_threshold, 58 | Features& fts) = 0; 59 | 60 | /// Flag the grid cell as occupied 61 | void setGridOccpuancy(const Vector2d& px); 62 | 63 | /// Set grid cells of existing features as occupied 64 | void setExistingFeatures(const Features& fts); 65 | 66 | protected: 67 | 68 | static const int border_ = 8; //!< no feature should be within 8px of border. 69 | const int cell_size_; 70 | const int n_pyr_levels_; 71 | const int grid_n_cols_; 72 | const int grid_n_rows_; 73 | vector grid_occupancy_; 74 | 75 | void resetGrid(); 76 | 77 | inline int getCellIndex(int x, int y, int level) 78 | { 79 | const int scale = (1< DetectorPtr; 84 | 85 | /// Edgelete detector 86 | class EdgeDetector : public AbstractDetector 87 | { 88 | public: 89 | EdgeDetector( 90 | const int img_width, 91 | const int img_height, 92 | const int cell_size, 93 | const int n_pyr_levels); 94 | 95 | virtual ~EdgeDetector() {} 96 | 97 | virtual void detect( 98 | Frame* frame, 99 | const ImgPyr& img_pyr, 100 | const double detection_threshold, 101 | Features& fts); 102 | }; 103 | 104 | /// grid size change detector, like dso 105 | class PixelSelector //: public AbstractDetector 106 | { 107 | public: 108 | int gridsize_= 32; 109 | int PixelNum_ = 300; 110 | int img_width_; 111 | int img_height_; 112 | int n_pyr_levels_; 113 | cv::Mat occpuancy_; // flag to show whethe the pixel has depth 114 | 115 | PixelSelector(int img_width, int img_height, int PixelNum,int n_pyr_levels); 116 | 117 | virtual ~PixelSelector() {} 118 | 119 | virtual void detect( 120 | Frame* frame, 121 | const ImgPyr& img_pyr, 122 | Features& fts); 123 | 124 | void setPixelOccpuancy(const Vector2d& px); 125 | void setExistingFeatures(const Features& fts); 126 | 127 | }; 128 | typedef boost::shared_ptr PixelSelectorPtr; 129 | // 130 | /// FAST detector by Edward Rosten. 131 | class FastDetector : public AbstractDetector 132 | { 133 | public: 134 | FastDetector( 135 | const int img_width, 136 | const int img_height, 137 | const int cell_size, 138 | const int n_pyr_levels); 139 | 140 | virtual ~FastDetector() {} 141 | 142 | virtual void detect( 143 | Frame* frame, 144 | const ImgPyr& img_pyr, 145 | const double detection_threshold, 146 | Features& fts); 147 | }; 148 | 149 | } // namespace feature_detection 150 | } // namespace svo 151 | 152 | #endif // SVO_FEATURE_DETECTION_H_ 153 | -------------------------------------------------------------------------------- /include/svo/frame.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FRAME_H_ 18 | #define SVO_FRAME_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace g2o { 27 | class VertexSE3Expmap; 28 | } 29 | typedef g2o::VertexSE3Expmap g2oFrameSE3; 30 | 31 | namespace svo { 32 | 33 | class Point; 34 | struct Feature; 35 | 36 | typedef list Features; 37 | typedef vector ImgPyr; 38 | 39 | /// A frame saves the image, the associated features and the estimated pose. 40 | class Frame : boost::noncopyable 41 | { 42 | public: 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | 45 | static int frame_counter_; //!< Counts the number of created frames. Used to set the unique id. 46 | int id_; //!< Unique id of the frame. 47 | double timestamp_; //!< Timestamp of when the image was recorded. 48 | svo::AbstractCamera* cam_; //!< Camera model. 49 | Sophus::SE3d T_f_w_; //!< Transform (f)rame from (w)orld. 50 | Eigen::Matrix Cov_; //!< Covariance. 51 | cv::Mat debug_img_; // used to draw feature in img_pyr_[0] 52 | ImgPyr img_pyr_; //!< Image Pyramid. 53 | Features fts_; //!< List of features in the image. 54 | vector key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. 55 | bool is_keyframe_; //!< Was this frames selected as keyframe? 56 | bool have_initializeSeeds; 57 | g2oFrameSE3* v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. 58 | int last_published_ts_; //!< Timestamp of last publishing. 59 | 60 | Frame(svo::AbstractCamera* cam, const cv::Mat& img, double timestamp); 61 | ~Frame(); 62 | 63 | /// Initialize new frame and create image pyramid. 64 | void initFrame(const cv::Mat& img); 65 | 66 | /// Select this frame as keyframe. 67 | void setKeyframe(); 68 | 69 | /// Add a feature to the image 70 | void addFeature(Feature* ftr); 71 | 72 | /// The KeyPoints are those five features which are closest to the 4 image corners 73 | /// and to the center and which have a 3D point assigned. These points are used 74 | /// to quickly check whether two frames have overlapping field of view. 75 | void setKeyPoints(); 76 | 77 | /// Check if we can select five better key-points. 78 | void checkKeyPoints(Feature* ftr); 79 | 80 | /// If a point is deleted, we must remove the corresponding key-point. 81 | void removeKeyPoint(Feature* ftr); 82 | 83 | /// Return number of point observations. 84 | inline size_t nObs() const { return fts_.size(); } 85 | 86 | /// Check if a point in (w)orld coordinate frame is visible in the image. 87 | bool isVisible(const Vector3d& xyz_w) const; 88 | 89 | /// Full resolution image stored in the frame. 90 | inline const cv::Mat& img() const { return img_pyr_[0]; } 91 | 92 | /// Was this frame selected as keyframe? 93 | inline bool isKeyframe() const { return is_keyframe_; } 94 | 95 | /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). 96 | inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); } 97 | 98 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). 99 | inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); } 100 | 101 | /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f). 102 | inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); } 103 | 104 | /// Transforms point coordinates in world-frame (w) to camera-frams (f). 105 | inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; } 106 | 107 | /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w). 108 | inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; } 109 | 110 | /// Projects Point from unit sphere (f) in camera pixels (c). 111 | inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); } 112 | 113 | /// Return the pose of the frame in the (w)orld coordinate frame. 114 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); } 115 | 116 | /// Frame jacobian for projection of 3D point in (f)rame coordinate to 117 | /// unit plane coordinates uv (focal length = 1). 118 | inline static void jacobian_xyz2uv( 119 | const Vector3d& xyz_in_f, 120 | Eigen::Matrix& J) 121 | { 122 | const double x = xyz_in_f[0]; 123 | const double y = xyz_in_f[1]; 124 | const double z_inv = 1./xyz_in_f[2]; 125 | const double z_inv_2 = z_inv*z_inv; 126 | 127 | J(0,0) = -z_inv; // -1/z 128 | J(0,1) = 0.0; // 0 129 | J(0,2) = x*z_inv_2; // x/z^2 130 | J(0,3) = y*J(0,2); // x*y/z^2 131 | J(0,4) = -(1.0 + x*J(0,2)); // -(1.0 + x^2/z^2) 132 | J(0,5) = y*z_inv; // y/z 133 | 134 | J(1,0) = 0.0; // 0 135 | J(1,1) = -z_inv; // -1/z 136 | J(1,2) = y*z_inv_2; // y/z^2 137 | J(1,3) = 1.0 + y*J(1,2); // 1.0 + y^2/z^2 138 | J(1,4) = -J(0,3); // -x*y/z^2 139 | J(1,5) = -x*z_inv; // x/z 140 | } 141 | }; 142 | 143 | 144 | /// Some helper functions for the frame object. 145 | namespace frame_utils { 146 | 147 | /// Creates an image pyramid of half-sampled images. 148 | void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); 149 | 150 | /// Get the average depth of the features in the image. 151 | bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); 152 | 153 | } // namespace frame_utils 154 | } // namespace svo 155 | 156 | #endif // SVO_FRAME_H_ 157 | -------------------------------------------------------------------------------- /include/svo/frame_handler_base.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FRAME_HANDLER_BASE_H_ 18 | #define SVO_FRAME_HANDLER_BASE_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | namespace svo 31 | { 32 | class Point; 33 | class Matcher; 34 | class DepthFilter; 35 | class AbstractCamera; 36 | class DeBuger; 37 | 38 | /// Base class for various VO pipelines. Manages the map and the state machine. 39 | class FrameHandlerBase : boost::noncopyable 40 | { 41 | public: 42 | enum Stage { 43 | STAGE_PAUSED, 44 | STAGE_FIRST_FRAME, 45 | STAGE_SECOND_FRAME, 46 | STAGE_DEFAULT_FRAME, 47 | STAGE_RELOCALIZING 48 | }; 49 | enum TrackingQuality { 50 | TRACKING_INSUFFICIENT, 51 | TRACKING_BAD, 52 | TRACKING_GOOD 53 | }; 54 | enum UpdateResult { 55 | RESULT_NO_KEYFRAME, 56 | RESULT_IS_KEYFRAME, 57 | RESULT_FAILURE 58 | }; 59 | 60 | FrameHandlerBase(); 61 | 62 | virtual ~FrameHandlerBase(); 63 | 64 | /// Get the current map. 65 | const Map& map() const { return map_; } 66 | 67 | /// Will reset the map as soon as the current frame is finished processing. 68 | void reset() { set_reset_ = true; } 69 | 70 | /// Start processing. 71 | void start() { set_start_ = true; } 72 | 73 | /// Get the current stage of the algorithm. 74 | Stage stage() const { return stage_; } 75 | 76 | /// Get tracking quality. 77 | TrackingQuality trackingQuality() const { return tracking_quality_; } 78 | 79 | /// Get the processing time of the previous iteration. 80 | double lastProcessingTime() const { return timer_.getTime(); } 81 | 82 | /// Get the number of feature observations of the last frame. 83 | size_t lastNumObservations() const { return num_obs_last_; } 84 | 85 | protected: 86 | Stage stage_; //!< Current stage of the algorithm. 87 | bool set_reset_; //!< Flag that the user can set. Will reset the system before the next iteration. 88 | bool set_start_; //!< Flag the user can set to start the system when the next image is received. 89 | Map map_; //!< Map of keyframes created by the slam system. 90 | svo::Timer timer_; //!< Stopwatch to measure time to process frame. 91 | svo::RingBuffer acc_frame_timings_; //!< Total processing time of the last 10 frames, used to give some user feedback on the performance. 92 | svo::RingBuffer acc_num_obs_; //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance. 93 | size_t num_obs_last_; //!< Number of observations in the previous frame. 94 | TrackingQuality tracking_quality_; //!< An estimate of the tracking quality based on the number of tracked features. 95 | 96 | /// Before a frame is processed, this function is called. 97 | bool startFrameProcessingCommon(const double timestamp); 98 | 99 | /// When a frame is finished processing, this function is called. 100 | int finishFrameProcessingCommon( 101 | const size_t update_id, 102 | const UpdateResult dropout, 103 | const size_t num_observations); 104 | 105 | /// Reset the map and frame handler to start from scratch. 106 | void resetCommon(); 107 | 108 | /// Reset the frame handler. Implement in derived class. 109 | virtual void resetAll() { resetCommon(); } 110 | 111 | /// Set the tracking quality based on the number of tracked features. 112 | virtual void setTrackingQuality(const size_t num_observations, const size_t init_match_number); 113 | 114 | /// Optimize some of the observed 3D points. 115 | virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter); 116 | }; 117 | 118 | } // namespace nslam 119 | 120 | #endif // SVO_FRAME_HANDLER_BASE_H_ 121 | -------------------------------------------------------------------------------- /include/svo/frame_handler_mono.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FRAME_HANDLER_H_ 18 | #define SVO_FRAME_HANDLER_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace svo { 28 | 29 | /// Monocular Visual Odometry Pipeline as described in the SVO paper. 30 | class FrameHandlerMono : public FrameHandlerBase 31 | { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | FrameHandlerMono(svo::AbstractCamera* cam); 36 | virtual ~FrameHandlerMono(); 37 | 38 | void Debug_show_img(); // used to debug reproject point 39 | 40 | /// Provide an image. 41 | void addImage(const cv::Mat& img, double timestamp); 42 | 43 | /// Set the first frame (used for synthetic datasets in benchmark node) 44 | void setFirstFrame(const FramePtr& first_frame); 45 | 46 | /// Get the last frame that has been processed. 47 | FramePtr lastFrame() { return last_frame_; } 48 | 49 | /// Get the set of spatially closest keyframes of the last frame. 50 | const set& coreKeyframes() { return core_kfs_; } 51 | const vector< pair > overlap_kfs(){return overlap_kfs_;} 52 | 53 | /// Return the feature track to visualize the KLT tracking during initialization. 54 | const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } 55 | const vector& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; } 56 | const vector& initFeatureTrackType() const { return klt_homography_init_.fts_type_; } 57 | 58 | /// Access the depth filter. 59 | DepthFilter* depthFilter() const { return depth_filter_; } 60 | 61 | 62 | /// An external place recognition module may know where to relocalize. 63 | bool relocalizeFrameAtPose( 64 | const int keyframe_id, 65 | const SE3d& T_kf_f, 66 | const cv::Mat& img, 67 | const double timestamp); 68 | 69 | protected: 70 | svo::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). 71 | //svo::PinholeCamera* cam_; 72 | Reprojector reprojector_; //!< Projects points from other keyframes into the current frame 73 | FramePtr new_frame_; //!< Current frame. 74 | FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. 75 | FramePtr last_kf_; // hyj: used to last_kf_ to judge the view changes, add new keyframe 76 | set core_kfs_; //!< Keyframes in the closer neighbourhood. 77 | vector< pair > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!? 78 | 79 | initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography. 80 | DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. 81 | 82 | /// Initialize the visual odometry algorithm. 83 | virtual void initialize(); 84 | 85 | /// Processes the first frame and sets it as a keyframe. 86 | virtual UpdateResult processFirstFrame(); 87 | 88 | /// Processes all frames after the first frame until a keyframe is selected. 89 | virtual UpdateResult processSecondFrame(); 90 | 91 | /// Processes all frames after the first two keyframes. 92 | virtual UpdateResult processFrame(); 93 | 94 | /// Try relocalizing the frame at relative position to provided keyframe. 95 | virtual UpdateResult relocalizeFrame( 96 | const SE3d& T_cur_ref, 97 | FramePtr ref_keyframe); 98 | 99 | /// Reset the frame handler. Implement in derived class. 100 | virtual void resetAll(); 101 | 102 | /// Keyframe selection criterion. 103 | virtual bool needNewKf(double scene_depth_mean); 104 | 105 | void setCoreKfs(size_t n_closest); 106 | 107 | }; 108 | 109 | } // namespace svo 110 | 111 | #endif // SVO_FRAME_HANDLER_H_ 112 | -------------------------------------------------------------------------------- /include/svo/global.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_GLOBAL_H_ 18 | #define SVO_GLOBAL_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | //#include 31 | #include 32 | #include 33 | 34 | #ifdef SVO_USE_ROS 35 | #include 36 | #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) 37 | #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x) 38 | #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x) 39 | #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) 40 | #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) 41 | #else 42 | #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc. 51 | #define SVO_WARN_STREAM_THROTTLE(rate, x) \ 52 | do { \ 53 | static double __log_stream_throttle__last_hit__ = 0.0; \ 54 | std::chrono::time_point __log_stream_throttle__now__ = \ 55 | std::chrono::system_clock::now(); \ 56 | if (__log_stream_throttle__last_hit__ + rate <= \ 57 | std::chrono::duration_cast( \ 58 | __log_stream_throttle__now__.time_since_epoch()).count()) { \ 59 | __log_stream_throttle__last_hit__ = \ 60 | std::chrono::duration_cast( \ 61 | __log_stream_throttle__now__.time_since_epoch()).count(); \ 62 | SVO_WARN_STREAM(x); \ 63 | } \ 64 | } while(0) 65 | #endif 66 | 67 | namespace svo 68 | { 69 | using namespace Eigen; 70 | using namespace Sophus; 71 | 72 | const double EPS = 0.0000000001; 73 | const double PI = 3.14159265; 74 | 75 | #ifdef SVO_TRACE 76 | extern svo::DeBuger* g_permon; 77 | #define SVO_LOG(value) g_permon->log(std::string((#value)),(value)) 78 | #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2) 79 | #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3) 80 | #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4) 81 | #define SVO_START_TIMER(name) g_permon->startTimer((name)) 82 | #define SVO_STOP_TIMER(name) g_permon->stopTimer((name)) 83 | #else 84 | #define SVO_LOG(v) 85 | #define SVO_LOG2(v1, v2) 86 | #define SVO_LOG3(v1, v2, v3) 87 | #define SVO_LOG4(v1, v2, v3, v4) 88 | #define SVO_START_TIMER(name) 89 | #define SVO_STOP_TIMER(name) 90 | #endif 91 | 92 | class Frame; 93 | typedef boost::shared_ptr FramePtr; 94 | } // namespace svo 95 | 96 | #endif // SVO_GLOBAL_H_ 97 | -------------------------------------------------------------------------------- /include/svo/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * homography.cpp 3 | * Adaptation of PTAM-GPL HomographyInit class. 4 | * https://github.com/Oxford-PTAM/PTAM-GPL 5 | * Licence: GPLv3 6 | * Copyright 2008 Isis Innovation Limited 7 | * 8 | * Created on: Sep 2, 2012 9 | * by: cforster 10 | * 11 | * This class implements the homography decomposition of Faugeras and Lustman's 12 | * 1988 tech report. Code converted to Eigen from PTAM. 13 | * 14 | */ 15 | 16 | #ifndef HOMOGRAPHY_H_ 17 | #define HOMOGRAPHY_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace svo { 25 | 26 | using namespace Eigen; 27 | using namespace std; 28 | 29 | struct HomographyDecomposition 30 | { 31 | Vector3d t; 32 | Matrix3d R; 33 | double d; 34 | Vector3d n; 35 | 36 | // Resolved Composition 37 | Sophus::SE3d T; //!< second from first 38 | int score; 39 | }; 40 | 41 | class Homography 42 | { 43 | public: 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | 46 | Homography (const vector >& _fts1, 47 | const vector >& _fts2, 48 | double _error_multiplier2, 49 | double _thresh_in_px); 50 | 51 | void 52 | calcFromPlaneParams (const Vector3d & normal, 53 | const Vector3d & point_on_plane); 54 | 55 | void 56 | calcFromMatches (); 57 | 58 | size_t 59 | computeMatchesInliers (); 60 | 61 | bool 62 | computeSE3fromMatches (); 63 | 64 | bool 65 | decompose (); 66 | 67 | void 68 | findBestDecomposition (); 69 | 70 | double thresh; 71 | double error_multiplier2; 72 | const vector >& fts_c1; //!< Features on first image on unit plane 73 | const vector >& fts_c2; //!< Features on second image on unit plane 74 | vector inliers; 75 | SE3d T_c2_from_c1; //!< Relative translation and rotation of two images 76 | Matrix3d H_c2_from_c1; //!< Homography 77 | vector decompositions; 78 | }; 79 | 80 | 81 | 82 | 83 | } /* end namespace vk */ 84 | 85 | #endif /* HOMOGRAPHY_H_ */ 86 | -------------------------------------------------------------------------------- /include/svo/initialization.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_INITIALIZATION_H 18 | #define SVO_INITIALIZATION_H 19 | 20 | #include 21 | 22 | namespace svo { 23 | 24 | class FrameHandlerMono; 25 | 26 | /// Bootstrapping the map from the first two views. 27 | namespace initialization { 28 | 29 | enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; 30 | 31 | 32 | /// Tracks features using Lucas-Kanade tracker and then estimates a homography. 33 | class KltHomographyInit { 34 | friend class svo::FrameHandlerMono; 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | FramePtr frame_ref_; 39 | KltHomographyInit() {}; 40 | ~KltHomographyInit() {}; 41 | InitResult addFirstFrame(FramePtr frame_ref); 42 | InitResult addSecondFrame(FramePtr frame_ref); 43 | void reset(); 44 | 45 | protected: 46 | vector px_ref_; //!< keypoints to be tracked in reference frame. 47 | vector px_cur_; //!< tracked keypoints in current frame. 48 | 49 | cv::Mat img_prev_; // hyj optical flow tracking frame by frame. 50 | vector px_prev_; // hyj optical flow tracking frame by frame. 51 | 52 | vector f_ref_; //!< bearing vectors corresponding to the keypoints in the reference image. 53 | vector fts_type_; // hyj : save feature type 54 | vector f_cur_; //!< bearing vectors corresponding to the keypoints in the current image. 55 | vector disparities_; //!< disparity between first and second frame. 56 | vector inliers_; //!< inliers after the geometric check (e.g., Homography). 57 | vector xyz_in_cur_; //!< 3D points computed during the geometric check. 58 | SE3d T_cur_from_ref_; //!< computed transformation between the first two frames. 59 | }; 60 | 61 | /// Detect Fast corners in the image. 62 | void detectFeatures(FramePtr frame, vector &fts_type, 63 | vector& px_vec, 64 | vector& f_vec); 65 | 66 | /// Compute optical flow (Lucas Kanade) for selected keypoints. 67 | /// 68 | /* 69 | void trackKlt(FramePtr frame_ref, 70 | FramePtr frame_cur, 71 | vector& px_ref, 72 | vector& px_cur, 73 | vector& f_ref, 74 | vector& f_cur, 75 | vector& disparities); 76 | */ 77 | void trackKlt(FramePtr frame_ref, 78 | FramePtr frame_cur, vector &fts_type, 79 | vector& px_ref, 80 | vector& px_cur, 81 | vector& f_ref, 82 | vector& f_cur, 83 | vector& disparities, cv::Mat & img_prev, vector & px_prev); 84 | 85 | void computeHomography(const vector& f_ref, 86 | const vector& f_cur, 87 | double focal_length, 88 | double reprojection_threshold, 89 | vector& inliers, 90 | vector& xyz_in_cur, 91 | SE3d& T_cur_from_ref, int method_choose); 92 | 93 | } // namespace initialization 94 | } // namespace nslam 95 | 96 | #endif // NSLAM_INITIALIZATION_H 97 | -------------------------------------------------------------------------------- /include/svo/map.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_MAP_H_ 18 | #define SVO_MAP_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace svo { 26 | 27 | class Point; 28 | class Feature; 29 | class Seed; 30 | 31 | /// Container for converged 3D points that are not already assigned to two keyframes. 32 | class MapPointCandidates 33 | { 34 | public: 35 | typedef std::pair PointCandidate; 36 | typedef std::list PointCandidateList; 37 | 38 | /// The depth-filter is running in a parallel thread and fills the canidate list. 39 | /// This mutex controls concurrent access to point_candidates. 40 | boost::mutex mut_; 41 | 42 | /// Candidate points are created from converged seeds. 43 | /// Until the next keyframe, these points can be used for reprojection and pose optimization. 44 | PointCandidateList candidates_; 45 | std::list< Point* > trash_points_; 46 | 47 | MapPointCandidates(); 48 | ~MapPointCandidates(); 49 | 50 | /// Add a candidate point. 51 | void newCandidatePoint(Point* point, double depth_sigma2); 52 | 53 | /// Adds the feature to the frame and deletes candidate from list. 54 | void addCandidatePointToFrame(FramePtr frame); 55 | 56 | /// Remove a candidate point from the list of candidates. 57 | bool deleteCandidatePoint(Point* point); 58 | 59 | /// Remove all candidates that belong to a frame. 60 | void removeFrameCandidates(FramePtr frame); 61 | 62 | /// Reset the candidate list, remove and delete all points. 63 | void reset(); 64 | 65 | void deleteCandidate(PointCandidate& c); 66 | 67 | void emptyTrash(); 68 | }; 69 | 70 | /// Map object which saves all keyframes which are in a map. 71 | class Map : boost::noncopyable 72 | { 73 | public: 74 | std::list< FramePtr > keyframes_; //!< List of keyframes in the map. 75 | std::list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also. 76 | MapPointCandidates point_candidates_; 77 | 78 | Map(); 79 | ~Map(); 80 | 81 | /// Reset the map. Delete all keyframes and reset the frame and point counters. 82 | void reset(); 83 | 84 | /// Delete a point in the map and remove all references in keyframes to it. 85 | void safeDeletePoint(Point* pt); 86 | 87 | /// Moves the point to the trash queue which is cleaned now and then. 88 | void deletePoint(Point* pt); 89 | 90 | /// Moves the frame to the trash queue which is cleaned now and then. 91 | bool safeDeleteFrame(FramePtr frame); 92 | 93 | /// Remove the references between a point and a frame. 94 | void removePtFrameRef(Frame* frame, Feature* ftr); 95 | 96 | /// Add a new keyframe to the map. 97 | void addKeyframe(FramePtr new_keyframe); 98 | 99 | /// Given a frame, return all keyframes which have an overlapping field of view. 100 | void getCloseKeyframes(const FramePtr& frame, std::list< std::pair >& close_kfs) const; 101 | 102 | /// Return the keyframe which is spatially closest and has overlapping field of view. 103 | FramePtr getClosestKeyframe(const FramePtr& frame) const; 104 | 105 | /// Return the keyframe which is furthest apart from pos. 106 | FramePtr getFurthestKeyframe(const Vector3d& pos) const; 107 | 108 | bool getKeyframeById(const int id, FramePtr& frame) const; 109 | 110 | /// Transform the whole map with rotation R, translation t and scale s. 111 | void transform(const Matrix3d& R, const Vector3d& t, const double& s); 112 | 113 | /// Empty trash bin of deleted keyframes and map points. We don't delete the 114 | /// points immediately to ensure proper cleanup and to provide the visualizer 115 | /// a list of objects which must be removed. 116 | void emptyTrash(); 117 | 118 | /// Return the keyframe which was last inserted in the map. 119 | inline FramePtr lastKeyframe() { return keyframes_.back(); } 120 | 121 | /// Return the number of keyframes in the map 122 | inline size_t size() const { return keyframes_.size(); } 123 | }; 124 | 125 | /// A collection of debug functions to check the data consistency. 126 | namespace map_debug { 127 | 128 | void mapStatistics(Map* map); 129 | void mapValidation(Map* map, int id); 130 | void frameValidation(Frame* frame, int id); 131 | void pointValidation(Point* point, int id); 132 | 133 | } // namespace map_debug 134 | } // namespace svo 135 | 136 | #endif // SVO_MAP_H_ 137 | -------------------------------------------------------------------------------- /include/svo/matcher.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_MATCHER_H_ 18 | #define SVO_MATCHER_H_ 19 | 20 | #include 21 | 22 | namespace vk { 23 | namespace patch_score { 24 | template class ZMSSD; 25 | } 26 | } 27 | 28 | namespace svo { 29 | 30 | class AbstractCamera; 31 | class Point; 32 | class Frame; 33 | class Feature; 34 | 35 | /// Warp a patch from the reference view to the current view. 36 | namespace warp { 37 | 38 | void getWarpMatrixAffine( 39 | const svo::AbstractCamera& cam_ref, 40 | const svo::AbstractCamera& cam_cur, 41 | const Vector2d& px_ref, 42 | const Vector3d& f_ref, 43 | const double depth_ref, 44 | const SE3d& T_cur_ref, 45 | const int level_ref, 46 | Matrix2d& A_cur_ref); 47 | 48 | int getBestSearchLevel( 49 | const Matrix2d& A_cur_ref, 50 | const int max_level); 51 | 52 | void warpAffine( 53 | const Matrix2d& A_cur_ref, 54 | const cv::Mat& img_ref, 55 | const Vector2d& px_ref, 56 | const int level_ref, 57 | const int level_cur, 58 | const int halfpatch_size, 59 | uint8_t* patch); 60 | 61 | void warpAffine( 62 | const svo::AbstractCamera& cam_ref, 63 | const Matrix2d& A_cur_ref, 64 | const cv::Mat& img_ref, 65 | const Vector2d& px_ref, 66 | const int level_ref, 67 | const int level_cur, 68 | const int halfpatch_size, 69 | uint8_t* patch); 70 | 71 | } // namespace warp 72 | 73 | /// Patch-matcher for reprojection-matching and epipolar search in triangulation. 74 | class Matcher 75 | { 76 | public: 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | 79 | int iii = 0; 80 | 81 | static const int halfpatch_size_ = 4; 82 | static const int patch_size_ = 8; 83 | 84 | typedef vk::patch_score::ZMSSD PatchScore; 85 | 86 | struct Options 87 | { 88 | bool align_1d; //!< in epipolar search: align patch 1D along epipolar line 89 | int align_max_iter; //!< number of iterations for aligning the feature patches in gauss newton 90 | double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align 91 | size_t max_epi_search_steps;//!< max number of evaluations along epipolar line 92 | bool subpix_refinement; //!< do gauss newton feature patch alignment after epipolar search 93 | bool epi_search_edgelet_filtering; 94 | double epi_search_edgelet_max_angle; 95 | Options() : 96 | align_1d(false), 97 | align_max_iter(10), 98 | max_epi_length_optim(2.0), 99 | max_epi_search_steps(1000), 100 | subpix_refinement(true), 101 | epi_search_edgelet_filtering(true), 102 | epi_search_edgelet_max_angle(0.7) 103 | {} 104 | } options_; 105 | 106 | uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16))); 107 | uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16))); 108 | Matrix2d A_cur_ref_; //!< affine warp matrix 109 | Vector2d epi_dir_; 110 | double epi_length_; //!< length of epipolar line segment in pixels (only used for epipolar search) 111 | double h_inv_; //!< hessian of 1d image alignment along epipolar line 112 | int search_level_; 113 | bool reject_; 114 | Feature* ref_ftr_; 115 | Vector2d px_cur_; 116 | 117 | Matcher() = default; 118 | ~Matcher() = default; 119 | 120 | /// Find a match by directly applying subpix refinement. 121 | /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result! 122 | bool findMatchDirect( 123 | const Point& pt, 124 | const Frame& frame, 125 | Vector2d& px_cur); 126 | 127 | /// Find a match by searching along the epipolar line without using any features. 128 | bool findEpipolarMatchDirect( 129 | const Frame& ref_frame, 130 | const Frame& cur_frame, 131 | const Feature& ref_ftr, 132 | const double d_estimate, 133 | const double d_min, 134 | const double d_max, 135 | double& depth); 136 | 137 | void createPatchFromPatchWithBorder(); 138 | }; 139 | 140 | } // namespace svo 141 | 142 | #endif // SVO_MATCHER_H_ 143 | -------------------------------------------------------------------------------- /include/svo/math_lib.h: -------------------------------------------------------------------------------- 1 | /* 2 | * math_utils.h 3 | * 4 | * Created on: Jul 20, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #ifndef MATH_LIB_H_ 9 | #define MATH_LIB_H_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace svo 17 | { 18 | 19 | using namespace Eigen; 20 | using namespace std; 21 | using namespace Sophus; 22 | 23 | Vector3d triangulateFeatureNonLin( 24 | const Matrix3d& R, 25 | const Vector3d& t, 26 | const Vector3d& feature1, 27 | const Vector3d& feature2); 28 | 29 | /// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. 30 | /// perfect triangulation without noise! 31 | bool depthFromTriangulationExact( 32 | const Matrix3d& R_r_c, 33 | const Vector3d& t_r_c, 34 | const Vector3d& f_r, 35 | const Vector3d& f_c, 36 | double& depth_in_r, 37 | double& depth_in_c); 38 | 39 | double reprojError( 40 | const Vector3d& f1, 41 | const Vector3d& f2, 42 | double error_multiplier2); 43 | 44 | double computeInliers( 45 | const vector& features1, 46 | const vector& features2, 47 | const Matrix3d& R, 48 | const Vector3d& t, 49 | const double reproj_thresh, 50 | double error_multiplier2, 51 | vector& xyz_vec, 52 | vector& inliers, 53 | vector& outliers); 54 | 55 | void computeInliersOneView( 56 | const vector & feature_sphere_vec, 57 | const vector & xyz_vec, 58 | const Matrix3d &R, 59 | const Vector3d &t, 60 | const double reproj_thresh, 61 | const double error_multiplier2, 62 | vector& inliers, 63 | vector& outliers); 64 | 65 | //! Direct Cosine Matrix to Roll Pitch Yaw 66 | Vector3d dcm2rpy(const Matrix3d &R); 67 | 68 | //! Roll Pitch Yaw to Direct Cosine Matrix 69 | Matrix3d rpy2dcm(const Vector3d &rpy); 70 | 71 | //! Angle Axis parametrization to Quaternion 72 | Quaterniond angax2quat(const Vector3d& n, const double& angle); 73 | 74 | //! Angle Axis parametrization to Matrix representation 75 | Matrix3d angax2dcm(const Vector3d& n, const double& angle); 76 | 77 | double sampsonusError( 78 | const Vector2d &v2Dash, 79 | const Matrix3d& m3Essential, 80 | const Vector2d& v2); 81 | 82 | inline Matrix3d sqew(const Vector3d& v) 83 | { 84 | Matrix3d v_sqew; 85 | v_sqew << 0, -v[2], v[1], 86 | v[2], 0, -v[0], 87 | -v[1], v[0], 0; 88 | return v_sqew; 89 | } 90 | 91 | inline double norm_max(const Eigen::VectorXd & v) 92 | { 93 | double max = -1; 94 | for (int i=0; imax){ 98 | max = abs; 99 | } 100 | } 101 | return max; 102 | } 103 | 104 | inline Vector2d project2d(const Vector3d& v) 105 | { 106 | return v.head<2>()/v[2]; 107 | } 108 | 109 | inline Vector3d unproject2d(const Vector2d& v) 110 | { 111 | return Vector3d(v[0], v[1], 1.0); 112 | } 113 | 114 | inline Vector3d project3d(const Vector4d& v) 115 | { 116 | return v.head<3>()/v[3]; 117 | } 118 | 119 | inline Vector4d unproject3d(const Vector3d& v) 120 | { 121 | return Vector4d(v[0], v[1], v[2], 1.0); 122 | } 123 | 124 | template 125 | T getMedian(vector& data_vec) 126 | { 127 | assert(!data_vec.empty()); 128 | typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); 129 | nth_element(data_vec.begin(), it, data_vec.end()); 130 | return *it; 131 | } 132 | 133 | inline double pyrFromZero_d(double x_0, int level) 134 | { 135 | return x_0/(1< & frame_jac) 148 | { 149 | const double x = xyz[0]; 150 | const double y = xyz[1]; 151 | const double z = xyz[2]; 152 | const double z_2 = z*z; 153 | 154 | frame_jac(0,0) = -1./z *focal_length; 155 | frame_jac(0,1) = 0; 156 | frame_jac(0,2) = x/z_2 *focal_length; 157 | frame_jac(0,3) = x*y/z_2 * focal_length; 158 | frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; 159 | frame_jac(0,5) = y/z *focal_length; 160 | 161 | frame_jac(1,0) = 0; 162 | frame_jac(1,1) = -1./z *focal_length; 163 | frame_jac(1,2) = y/z_2 *focal_length; 164 | frame_jac(1,3) = (1+y*y/z_2) *focal_length; 165 | frame_jac(1,4) = -x*y/z_2 *focal_length; 166 | frame_jac(1,5) = -x/z *focal_length; 167 | } 168 | 169 | } // end namespace vk 170 | 171 | #endif /* MATH_UTILS_H_ */ 172 | 173 | -------------------------------------------------------------------------------- /include/svo/point.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_POINT_H_ 18 | #define SVO_POINT_H_ 19 | 20 | #include 21 | #include 22 | 23 | namespace g2o { 24 | class VertexSBAPointXYZ; 25 | } 26 | typedef g2o::VertexSBAPointXYZ g2oPoint; 27 | 28 | namespace svo { 29 | 30 | class Feature; 31 | 32 | typedef Eigen::Matrix Matrix23d; 33 | 34 | /// A 3D point on the surface of the scene. 35 | class Point : boost::noncopyable 36 | { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | enum PointType { 41 | TYPE_DELETED, 42 | TYPE_CANDIDATE, 43 | TYPE_UNKNOWN, 44 | TYPE_GOOD 45 | }; 46 | 47 | static int point_counter_; //!< Counts the number of created points. Used to set the unique id. 48 | int id_; //!< Unique ID of the point. 49 | Vector3d pos_; //!< 3d pos of the point in the world coordinate frame. 50 | Vector3d normal_; //!< Surface normal at point. 51 | Matrix3d normal_information_; //!< Inverse covariance matrix of normal estimation. 52 | bool normal_set_; //!< Flag whether the surface normal was estimated or not. 53 | std::list obs_; //!< References to keyframes which observe the point. 54 | size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. 55 | g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. 56 | int last_published_ts_; //!< Timestamp of last publishing. 57 | int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. 58 | PointType type_; //!< Quality of the point. 59 | int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. 60 | int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. 61 | int last_structure_optim_; //!< Timestamp of last point optimization 62 | 63 | Point(const Vector3d& pos); 64 | Point(const Vector3d& pos, Feature* ftr); 65 | ~Point(); 66 | 67 | /// Add a reference to a frame. 68 | void addFrameRef(Feature* ftr); 69 | 70 | /// Remove reference to a frame. 71 | bool deleteFrameRef(Frame* frame); 72 | 73 | /// Initialize point normal. The inital estimate will point towards the frame. 74 | void initNormal(); 75 | 76 | /// Check whether mappoint has reference to a frame. 77 | Feature* findFrameRef(Frame* frame); 78 | 79 | /// Get Frame with similar viewpoint. 80 | bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const; 81 | 82 | /// Get number of observations. 83 | inline size_t nRefs() const { return obs_.size(); } 84 | 85 | /// Optimize point position through minimizing the reprojection error. 86 | void optimize(const size_t n_iter); 87 | 88 | /// Jacobian of point projection on unit plane (focal length = 1) in frame (f). 89 | inline static void jacobian_xyz2uv( 90 | const Vector3d& p_in_f, 91 | const Matrix3d& R_f_w, 92 | Matrix23d& point_jac) 93 | { 94 | const double z_inv = 1.0/p_in_f[2]; 95 | const double z_inv_sq = z_inv*z_inv; 96 | point_jac(0, 0) = z_inv; 97 | point_jac(0, 1) = 0.0; 98 | point_jac(0, 2) = -p_in_f[0] * z_inv_sq; 99 | point_jac(1, 0) = 0.0; 100 | point_jac(1, 1) = z_inv; 101 | point_jac(1, 2) = -p_in_f[1] * z_inv_sq; 102 | point_jac = - point_jac * R_f_w; 103 | } 104 | }; 105 | 106 | } // namespace svo 107 | 108 | #endif // SVO_POINT_H_ 109 | -------------------------------------------------------------------------------- /include/svo/pose_optimizer.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_POSE_OPTIMIZER_H_ 18 | #define SVO_POSE_OPTIMIZER_H_ 19 | 20 | #include 21 | 22 | namespace svo { 23 | 24 | using namespace Eigen; 25 | using namespace Sophus; 26 | using namespace std; 27 | 28 | typedef Eigen::Matrix Matrix6d; 29 | typedef Eigen::Matrix Matrix26d; 30 | typedef Eigen::Matrix Matrix16d; 31 | typedef Eigen::Matrix Vector6d; 32 | 33 | class Point; 34 | 35 | /// Motion-only bundle adjustment. Minimize the reprojection error of a single frame. 36 | namespace pose_optimizer { 37 | 38 | void optimizeGaussNewton( 39 | const double reproj_thresh, 40 | const size_t n_iter, 41 | const bool verbose, 42 | FramePtr& frame, 43 | double& estimated_scale, 44 | double& error_init, 45 | double& error_final, 46 | size_t& num_obs); 47 | 48 | } // namespace pose_optimizer 49 | } // namespace svo 50 | 51 | #endif // SVO_POSE_OPTIMIZER_H_ 52 | -------------------------------------------------------------------------------- /include/svo/reprojector.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_REPROJECTION_H_ 18 | #define SVO_REPROJECTION_H_ 19 | 20 | #include 21 | #include 22 | 23 | namespace svo { 24 | class AbstractCamera; 25 | } 26 | 27 | namespace svo { 28 | 29 | class Map; 30 | class Point; 31 | 32 | /// Project points from the map into the image and find the corresponding 33 | /// feature (corner). We don't search a match for every point but only for one 34 | /// point per cell. Thereby, we achieve a homogeneously distributed set of 35 | /// matched features and at the same time we can save processing time by not 36 | /// projecting all points. 37 | class Reprojector 38 | { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | /// Reprojector config parameters 43 | struct Options { 44 | size_t max_n_kfs; //!< max number of keyframes to reproject from 45 | bool find_match_direct; 46 | Options() 47 | : max_n_kfs(10), 48 | find_match_direct(true) 49 | {} 50 | } options_; 51 | 52 | size_t n_matches_; 53 | size_t n_trials_; 54 | 55 | Reprojector(svo::AbstractCamera* cam, Map& map); 56 | 57 | ~Reprojector(); 58 | 59 | /// Project points from the map into the image. First finds keyframes with 60 | /// overlapping field of view and projects only those map-points. 61 | void reprojectMap( 62 | FramePtr frame, 63 | std::vector< std::pair >& overlap_kfs); 64 | 65 | private: 66 | 67 | /// A candidate is a point that projects into the image plane and for which we 68 | /// will search a maching feature in the image. 69 | struct Candidate { 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | Point* pt; //!< 3D point. 72 | Vector2d px; //!< projected 2D pixel location. 73 | Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} 74 | }; 75 | typedef std::list > Cell; 76 | typedef std::vector CandidateGrid; 77 | 78 | /// The grid stores a set of candidate matches. For every grid cell we try to find one match. 79 | struct Grid 80 | { 81 | CandidateGrid cells; 82 | std::vector cell_order; 83 | int cell_size; 84 | int grid_n_cols; 85 | int grid_n_rows; 86 | }; 87 | 88 | Grid grid_; 89 | Matcher matcher_; 90 | Map& map_; 91 | 92 | static bool pointQualityComparator(Candidate& lhs, Candidate& rhs); 93 | void initializeGrid(svo::AbstractCamera* cam); 94 | void resetGrid(); 95 | bool reprojectCell(Cell& cell, FramePtr frame); 96 | bool reprojectPoint(FramePtr frame, Point* point); 97 | }; 98 | 99 | } // namespace svo 100 | 101 | #endif // SVO_REPROJECTION_H_ 102 | -------------------------------------------------------------------------------- /include/svo/ringbuffer.h: -------------------------------------------------------------------------------- 1 | // This file is part of VisionTools. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 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 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell 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 13 | // all 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 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef VISIONTOOLS_RING_BUFFER_H 24 | #define VISIONTOOLS_RING_BUFFER_H 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace svo 31 | { 32 | 33 | template 34 | class RingBuffer 35 | { 36 | public: 37 | RingBuffer (int size); 38 | 39 | void 40 | push_back (const T & elem); 41 | 42 | bool 43 | empty () const; 44 | 45 | T 46 | get (int i); 47 | 48 | T 49 | getSum () const; 50 | 51 | T 52 | getMean () const; 53 | 54 | int size() 55 | { 56 | return num_elem_; 57 | } 58 | 59 | private: 60 | std::vector arr_; 61 | int begin_; 62 | int end_; 63 | int num_elem_; 64 | int arr_size_; 65 | }; 66 | 67 | template 68 | RingBuffer 69 | ::RingBuffer(int size) : 70 | arr_(size), 71 | begin_(0), 72 | end_(-1), 73 | num_elem_(0), 74 | arr_size_(size) 75 | {} 76 | 77 | template 78 | bool RingBuffer 79 | ::empty() const 80 | { 81 | return arr_.empty(); 82 | } 83 | 84 | template 85 | void RingBuffer 86 | ::push_back(const T & elem) 87 | { 88 | if (num_elem_ 102 | T RingBuffer 103 | ::get(int i) 104 | { 105 | assert(i 110 | T RingBuffer 111 | ::getSum() const 112 | { 113 | T sum=0; 114 | for(int i=0; i 120 | T RingBuffer 121 | ::getMean() const 122 | { 123 | if(num_elem_ == 0) 124 | return 0; 125 | return getSum()/num_elem_; 126 | } 127 | 128 | } 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /include/svo/robust_cost.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of dvo. 3 | * 4 | * Copyright 2012 Christian Kerl (Technical University of Munich) 5 | * For more information see . 6 | * 7 | * dvo 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 | * dvo 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 dvo. If not, see . 19 | */ 20 | 21 | #ifndef VIKIT_ROBUST_COST_H_ 22 | #define VIKIT_ROBUST_COST_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | namespace svo { 28 | namespace robust_cost { 29 | 30 | // interface for scale estimators 31 | class ScaleEstimator 32 | { 33 | public: 34 | virtual ~ScaleEstimator() {}; 35 | virtual float compute(std::vector& errors) const = 0; 36 | }; 37 | typedef std::shared_ptr ScaleEstimatorPtr; 38 | 39 | class UnitScaleEstimator : public ScaleEstimator 40 | { 41 | public: 42 | UnitScaleEstimator() {} 43 | virtual ~UnitScaleEstimator() {} 44 | virtual float compute(std::vector& errors) const { return 1.0f; }; 45 | }; 46 | 47 | // estimates scale by fitting a t-distribution to the data with the given degrees of freedom 48 | class TDistributionScaleEstimator : public ScaleEstimator 49 | { 50 | public: 51 | TDistributionScaleEstimator(const float dof = DEFAULT_DOF); 52 | virtual ~TDistributionScaleEstimator() {}; 53 | virtual float compute(std::vector& errors) const; 54 | 55 | static const float DEFAULT_DOF; 56 | static const float INITIAL_SIGMA; 57 | protected: 58 | float dof_; 59 | float initial_sigma_; 60 | }; 61 | 62 | // estimates scale by computing the median absolute deviation 63 | class MADScaleEstimator : public ScaleEstimator 64 | { 65 | public: 66 | MADScaleEstimator() {}; 67 | virtual ~MADScaleEstimator() {}; 68 | virtual float compute(std::vector& errors) const; 69 | 70 | private: 71 | static const float NORMALIZER;; 72 | }; 73 | 74 | // estimates scale by computing the standard deviation 75 | class NormalDistributionScaleEstimator : public ScaleEstimator 76 | { 77 | public: 78 | NormalDistributionScaleEstimator() {}; 79 | virtual ~NormalDistributionScaleEstimator() {}; 80 | virtual float compute(std::vector& errors) const; 81 | private: 82 | }; 83 | 84 | /** 85 | * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). 86 | * The errors are assumed to be normalized to unit variance. 87 | * 88 | * See: 89 | * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) 90 | */ 91 | class WeightFunction 92 | { 93 | public: 94 | virtual ~WeightFunction() {}; 95 | virtual float value(const float& x) const = 0; 96 | virtual void configure(const float& param) {}; 97 | }; 98 | typedef std::shared_ptr WeightFunctionPtr; 99 | 100 | class UnitWeightFunction : public WeightFunction 101 | { 102 | public: 103 | UnitWeightFunction() {}; 104 | virtual ~UnitWeightFunction() {}; 105 | virtual float value(const float& x) const { return 1.0f; }; 106 | }; 107 | 108 | /** 109 | * Tukey's hard re-descending function. 110 | * 111 | * See: 112 | * http://en.wikipedia.org/wiki/Redescending_M-estimator 113 | */ 114 | class TukeyWeightFunction : public WeightFunction 115 | { 116 | public: 117 | TukeyWeightFunction(const float b = DEFAULT_B); 118 | virtual ~TukeyWeightFunction() {}; 119 | virtual float value(const float& x) const; 120 | virtual void configure(const float& param); 121 | 122 | static const float DEFAULT_B; 123 | private: 124 | float b_square; 125 | }; 126 | 127 | class TDistributionWeightFunction : public WeightFunction 128 | { 129 | public: 130 | TDistributionWeightFunction(const float dof = DEFAULT_DOF); 131 | virtual ~TDistributionWeightFunction() {}; 132 | virtual float value(const float& x) const; 133 | virtual void configure(const float& param); 134 | 135 | static const float DEFAULT_DOF; 136 | private: 137 | float dof_; 138 | float normalizer_; 139 | }; 140 | 141 | class HuberWeightFunction : public WeightFunction 142 | { 143 | public: 144 | HuberWeightFunction(const float k = DEFAULT_K); 145 | virtual ~HuberWeightFunction() {}; 146 | virtual float value(const float& x) const; 147 | virtual void configure(const float& param); 148 | 149 | static const float DEFAULT_K; 150 | private: 151 | float k; 152 | }; 153 | 154 | } // namespace robust_cost 155 | } // namespace vk 156 | #endif // VIKIT_ROBUST_COST_H_ 157 | -------------------------------------------------------------------------------- /include/svo/slamviewer.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAMVIEWER_H 2 | #define SLAMVIEWER_H 3 | 4 | #include "opencv2/core/core.hpp" 5 | #include 6 | 7 | #include "pangolin/pangolin.h" 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | namespace SLAM_VIEWER { 15 | 16 | class Viewer 17 | { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | Viewer( svo::FrameHandlerMono* vo); 22 | ~Viewer(); 23 | void run(); 24 | bool CheckFinish(); 25 | void DrawKeyFrames(const bool bDrawKF); 26 | void DrawMapRegionPoints(); 27 | void DrawMapSeeds(); 28 | 29 | void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M); 30 | void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc); 31 | 32 | private: 33 | svo::FrameHandlerMono* _vo; 34 | 35 | std::mutex mMutexCurrentPose; 36 | std::vector< Sophus::SE3d > _pos; 37 | Sophus::SE3d _CurrentPoseTwc ; 38 | int _drawedframeID=0; 39 | 40 | void SetFinish(); 41 | bool mbFinished; 42 | std::mutex mMutexFinish; 43 | 44 | bool CheckFinish2(); 45 | bool mbFinished2; 46 | std::mutex mMutexFinish2; 47 | 48 | float mKeyFrameSize; 49 | float mKeyFrameLineWidth; 50 | float mGraphLineWidth; 51 | float mPointSize; 52 | float mCameraSize; 53 | float mCameraLineWidth; 54 | 55 | float mViewpointX, mViewpointY, mViewpointZ, mViewpointF; 56 | 57 | }; 58 | 59 | } 60 | #endif // SLAMVIEWER_H 61 | -------------------------------------------------------------------------------- /include/svo/sparse_align.h: -------------------------------------------------------------------------------- 1 | #ifndef SPARSE_ALIGN_H 2 | #define SPARSE_ALIGN_H 3 | 4 | //#include 5 | //#include 6 | #include 7 | #define D 6 8 | namespace svo { 9 | 10 | class AbstractCamera; 11 | class Feature; 12 | 13 | /// Optimize the pose of the frame by minimizing the photometric error of feature patches. 14 | class SparseAlign 15 | { 16 | static const int patch_halfsize_ = 2; 17 | static const int patch_size_ = 2*patch_halfsize_; 18 | static const int patch_area_ = patch_size_*patch_size_; // 16 pixels 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | cv::Mat resimg_; 24 | 25 | SparseAlign( 26 | int n_levels, 27 | int min_level, 28 | int n_iter, 29 | //Method method, 30 | bool display, 31 | bool verbose); 32 | 33 | size_t run( 34 | FramePtr ref_frame, 35 | FramePtr cur_frame); 36 | 37 | /// Return fisher information matrix, i.e. the Hessian of the log-likelihood 38 | /// at the converged state. 39 | Eigen::Matrix getFisherInformation(); 40 | 41 | protected: 42 | FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels. 43 | FramePtr cur_frame_; //!< only the image is known! 44 | int level_; //!< current pyramid level on which the optimization runs. 45 | bool display_; //!< display residual image. 46 | int max_level_; //!< coarsest pyramid level for the alignment. 47 | int min_level_; //!< finest pyramid level for the alignment. 48 | 49 | Eigen::Matrix H_; //!< Hessian approximation 50 | Eigen::Matrix Jres_; //!< Jacobian x Residual 51 | Eigen::Matrix x_; //!< update step 52 | 53 | size_t n_iter_init_, n_iter_; //!< Number of Iterations 54 | size_t n_meas_; //!< Number of measurements 55 | bool stop_; //!< Stop flag 56 | bool verbose_; //!< Output Statistics 57 | double eps_; //!< Stop if update norm is smaller than eps 58 | size_t iter_; //!< Current Iteration 59 | 60 | double chi2_; 61 | double rho_; 62 | 63 | // robust least squares 64 | bool use_weights_; 65 | float scale_; 66 | //robust_cost::ScaleEstimatorPtr scale_estimator_; 67 | //robust_cost::WeightFunctionPtr weight_function_; 68 | 69 | 70 | // cache: 71 | Eigen::Matrix jacobian_cache_; 72 | bool have_ref_patch_cache_; 73 | cv::Mat ref_patch_cache_; 74 | std::vector visible_fts_; 75 | 76 | void precomputeReferencePatches(); 77 | void optimize(SE3d& model); 78 | void reset(); 79 | double computeResiduals(const SE3d& model, bool linearize_system, bool compute_weight_scale = false); 80 | int solve(); 81 | void update (const SE3d& old_model, SE3d& new_model); 82 | void startIteration(); 83 | void finishIteration(); 84 | }; 85 | 86 | } // namespace svo 87 | 88 | #endif // SPARSE_ALIGN_H 89 | -------------------------------------------------------------------------------- /include/svo/sparse_img_align.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_SPARSE_IMG_ALIGN_H_ 18 | #define SVO_SPARSE_IMG_ALIGN_H_ 19 | /* 20 | #include 21 | #include 22 | #include 23 | 24 | namespace svo { 25 | 26 | class AbstractCamera; 27 | class Feature; 28 | 29 | /// Optimize the pose of the frame by minimizing the photometric error of feature patches. 30 | class SparseImgAlign : public vk::NLLSSolver<6, SE3> 31 | { 32 | static const int patch_halfsize_ = 2; 33 | static const int patch_size_ = 2*patch_halfsize_; 34 | static const int patch_area_ = patch_size_*patch_size_; 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | cv::Mat resimg_; 39 | 40 | SparseImgAlign( 41 | int n_levels, 42 | int min_level, 43 | int n_iter, 44 | Method method, 45 | bool display, 46 | bool verbose); 47 | 48 | size_t run( 49 | FramePtr ref_frame, 50 | FramePtr cur_frame); 51 | 52 | /// Return fisher information matrix, i.e. the Hessian of the log-likelihood 53 | /// at the converged state. 54 | Matrix getFisherInformation(); 55 | 56 | protected: 57 | FramePtr ref_frame_; //!< reference frame, has depth for gradient pixels. 58 | FramePtr cur_frame_; //!< only the image is known! 59 | int level_; //!< current pyramid level on which the optimization runs. 60 | bool display_; //!< display residual image. 61 | int max_level_; //!< coarsest pyramid level for the alignment. 62 | int min_level_; //!< finest pyramid level for the alignment. 63 | 64 | // cache: 65 | Matrix jacobian_cache_; 66 | bool have_ref_patch_cache_; 67 | cv::Mat ref_patch_cache_; 68 | std::vector visible_fts_; 69 | 70 | void precomputeReferencePatches(); 71 | virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false); 72 | virtual int solve(); 73 | virtual void update (const ModelType& old_model, ModelType& new_model); 74 | virtual void startIteration(); 75 | virtual void finishIteration(); 76 | }; 77 | 78 | } // namespace svo 79 | */ 80 | #endif // SVO_SPARSE_IMG_ALIGN_H_ 81 | -------------------------------------------------------------------------------- /lib/libsvo.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HeYijia/svo_edgelet/1f408d7c3c10d3a28184fd84d199d757e46af672/lib/libsvo.so -------------------------------------------------------------------------------- /src/camera_model.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pinhole_camera.cpp 3 | * 4 | * Created on: Jul 24, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace svo { 17 | 18 | PinholeCamera:: 19 | PinholeCamera(double width, double height, 20 | double fx, double fy, 21 | double cx, double cy, 22 | double d0, double d1, double d2, double d3, double d4) : 23 | AbstractCamera(width, height), 24 | fx_(fx), fy_(fy), cx_(cx), cy_(cy), 25 | distortion_(fabs(d0) > 0.0000001), 26 | undist_map1_(height_, width_, CV_16SC2), 27 | undist_map2_(height_, width_, CV_16SC2), 28 | use_optimization_(false) 29 | { 30 | d_[0] = d0; d_[1] = d1; d_[2] = d2; d_[3] = d3; d_[4] = d4; 31 | cvK_ = (cv::Mat_(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0); 32 | cvD_ = (cv::Mat_(1, 5) << d_[0], d_[1], d_[2], d_[3], d_[4]); 33 | cv::initUndistortRectifyMap(cvK_, cvD_, cv::Mat_::eye(3,3), cvK_, 34 | cv::Size(width_, height_), CV_16SC2, undist_map1_, undist_map2_); 35 | K_ << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0; 36 | K_inv_ = K_.inverse(); 37 | } 38 | 39 | PinholeCamera:: 40 | ~PinholeCamera() 41 | {} 42 | 43 | Vector3d PinholeCamera:: 44 | cam2world(const double& u, const double& v) const 45 | { 46 | Vector3d xyz; 47 | if(!distortion_) 48 | { 49 | xyz[0] = (u - cx_)/fx_; 50 | xyz[1] = (v - cy_)/fy_; 51 | xyz[2] = 1.0; 52 | } 53 | else 54 | { 55 | cv::Point2f uv(u,v), px; 56 | const cv::Mat src_pt(1, 1, CV_32FC2, &uv.x); 57 | cv::Mat dst_pt(1, 1, CV_32FC2, &px.x); 58 | cv::undistortPoints(src_pt, dst_pt, cvK_, cvD_); 59 | xyz[0] = px.x; 60 | xyz[1] = px.y; 61 | xyz[2] = 1.0; 62 | } 63 | return xyz.normalized(); 64 | } 65 | 66 | Vector2d PinholeCamera:: 67 | undistortpoint(const double& u, const double& v) const 68 | { 69 | Vector2d px; 70 | if(!distortion_) 71 | { 72 | px[0] = u; 73 | px[1] = v; 74 | } 75 | else 76 | { 77 | double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; 78 | x = (u - cx_)/fx_; 79 | y = (v - cy_)/fy_; 80 | r2 = x*x + y*y; 81 | r4 = r2*r2; 82 | r6 = r4*r2; 83 | a1 = 2*x*y; 84 | a2 = r2 + 2*x*x; 85 | a3 = r2 + 2*y*y; 86 | cdist = 1 + d_[0]*r2 + d_[1]*r4 + d_[4]*r6; 87 | xd = x*cdist + d_[2]*a1 + d_[3]*a2; 88 | yd = y*cdist + d_[2]*a3 + d_[3]*a1; 89 | px[0] = xd*fx_ + cx_; 90 | px[1] = yd*fy_ + cy_; 91 | } 92 | return px; 93 | } 94 | 95 | Vector3d PinholeCamera:: 96 | cam2world (const Vector2d& uv) const 97 | { 98 | return cam2world(uv[0], uv[1]); 99 | } 100 | 101 | Vector2d PinholeCamera:: 102 | world2cam(const Vector3d& xyz) const 103 | { 104 | //return world2cam(vk::project2d(xyz)); 105 | Vector2d uv= xyz.head<2>()/xyz[2]; 106 | return world2cam(uv); 107 | } 108 | 109 | Vector2d PinholeCamera:: 110 | world2cam(const Vector2d& uv) const 111 | { 112 | Vector2d px; 113 | if(!distortion_) 114 | { 115 | px[0] = fx_*uv[0] + cx_; 116 | px[1] = fy_*uv[1] + cy_; 117 | } 118 | else 119 | { 120 | double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; 121 | x = uv[0]; 122 | y = uv[1]; 123 | r2 = x*x + y*y; 124 | r4 = r2*r2; 125 | r6 = r4*r2; 126 | a1 = 2*x*y; 127 | a2 = r2 + 2*x*x; 128 | a3 = r2 + 2*y*y; 129 | cdist = 1 + d_[0]*r2 + d_[1]*r4 + d_[4]*r6; 130 | xd = x*cdist + d_[2]*a1 + d_[3]*a2; 131 | yd = y*cdist + d_[2]*a3 + d_[3]*a1; 132 | px[0] = xd*fx_ + cx_; 133 | px[1] = yd*fy_ + cy_; 134 | } 135 | return px; 136 | } 137 | 138 | void PinholeCamera:: 139 | undistortImage(const cv::Mat& raw, cv::Mat& rectified) 140 | { 141 | if(distortion_) 142 | cv::remap(raw, rectified, undist_map1_, undist_map2_, CV_INTER_LINEAR); 143 | else 144 | rectified = raw.clone(); 145 | } 146 | 147 | } // end namespace vk 148 | -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifdef SVO_USE_ROS 18 | #include 19 | #endif 20 | #include 21 | 22 | namespace svo { 23 | 24 | Config::Config() : 25 | #ifdef SVO_USE_ROS 26 | trace_name(vk::getParam("svo/trace_name", "svo")), 27 | trace_dir(vk::getParam("svo/trace_dir", "/tmp")), 28 | n_pyr_levels(vk::getParam("svo/n_pyr_levels", 3)), 29 | use_imu(vk::getParam("svo/use_imu", false)), 30 | core_n_kfs(vk::getParam("svo/core_n_kfs", 3)), 31 | map_scale(vk::getParam("svo/map_scale", 1.0)), 32 | grid_size(vk::getParam("svo/grid_size", 30)), 33 | init_min_disparity(vk::getParam("svo/init_min_disparity", 50.0)), 34 | init_min_tracked(vk::getParam("svo/init_min_tracked", 50)), 35 | init_min_inliers(vk::getParam("svo/init_min_inliers", 40)), 36 | klt_max_level(vk::getParam("svo/klt_max_level", 4)), 37 | klt_min_level(vk::getParam("svo/klt_min_level", 2)), 38 | reproj_thresh(vk::getParam("svo/reproj_thresh", 2.0)), 39 | poseoptim_thresh(vk::getParam("svo/poseoptim_thresh", 2.0)), 40 | poseoptim_num_iter(vk::getParam("svo/poseoptim_num_iter", 10)), 41 | structureoptim_max_pts(vk::getParam("svo/structureoptim_max_pts", 20)), 42 | structureoptim_num_iter(vk::getParam("svo/structureoptim_num_iter", 5)), 43 | loba_thresh(vk::getParam("svo/loba_thresh", 2.0)), 44 | loba_robust_huber_width(vk::getParam("svo/loba_robust_huber_width", 1.0)), 45 | loba_num_iter(vk::getParam("svo/loba_num_iter", 0)), 46 | kfselect_mindist(vk::getParam("svo/kfselect_mindist", 0.12)), 47 | triang_min_corner_score(vk::getParam("svo/triang_min_corner_score", 20.0)), 48 | triang_half_patch_size(vk::getParam("svo/triang_half_patch_size", 4)), 49 | subpix_n_iter(vk::getParam("svo/subpix_n_iter", 10)), 50 | max_n_kfs(vk::getParam("svo/max_n_kfs", 10)), 51 | img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), 52 | max_fts(vk::getParam("svo/max_fts", 120)), 53 | quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), 54 | quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)) 55 | #else 56 | trace_name("svo"), 57 | trace_dir("/tmp"), 58 | n_pyr_levels(3), //3 59 | use_imu(false), 60 | core_n_kfs(3), 61 | map_scale(1.0), 62 | grid_size(25), // 75 63 | init_min_disparity(20.0), 64 | init_min_tracked(50), 65 | init_min_inliers(40), 66 | klt_max_level(4), // 4 //6 67 | klt_min_level(2), // 2 //4 68 | reproj_thresh(2.0), // 2 //6.0 69 | poseoptim_thresh(2.0), 70 | poseoptim_num_iter(10), 71 | structureoptim_max_pts(20), 72 | structureoptim_num_iter(5), 73 | loba_thresh(2.0), 74 | loba_robust_huber_width(1.0), 75 | loba_num_iter(0), 76 | kfselect_mindist(0.12), // 0.12 77 | triang_min_corner_score(5.0), // 20 78 | triang_half_patch_size(4), 79 | subpix_n_iter(10), 80 | max_n_kfs(30), // 10 81 | img_imu_delay(0.0), 82 | max_fts(180), //120 83 | quality_min_fts(30), //50 //20 84 | quality_max_drop_fts(40) 85 | #endif 86 | {} 87 | 88 | Config& Config::getInstance() 89 | { 90 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 91 | return instance; 92 | } 93 | 94 | } // namespace svo 95 | 96 | -------------------------------------------------------------------------------- /src/debug.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DeBuger.cpp 3 | * 4 | * Created on: Aug 26, 2011 5 | * Author: Christian Forster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace svo 13 | { 14 | using namespace std; 15 | 16 | DeBuger::DeBuger() 17 | {} 18 | 19 | DeBuger::~DeBuger() 20 | { 21 | ofs_.flush(); 22 | ofs_.close(); 23 | } 24 | 25 | void DeBuger::init( 26 | const string& trace_name, 27 | const string& trace_dir) 28 | { 29 | trace_name_ = trace_name; 30 | trace_dir_ = trace_dir; 31 | string filename(trace_dir + "/" + trace_name + ".csv"); 32 | ofs_.open(filename.c_str()); 33 | if(!ofs_.is_open()) 34 | { 35 | printf("Tracefile = %s\n", filename.c_str()); 36 | throw runtime_error("Could not open tracefile."); 37 | } 38 | traceHeader(); 39 | } 40 | 41 | void DeBuger::addTimer(const string& name) 42 | { 43 | timers_.insert(make_pair(name, Timer())); 44 | } 45 | 46 | void DeBuger::addLog(const string& name) 47 | { 48 | logs_.insert(make_pair(name, LogItem())); 49 | } 50 | 51 | void DeBuger::writeToFile() 52 | { 53 | trace(); 54 | 55 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 56 | it->second.reset(); 57 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 58 | { 59 | it->second.set = false; 60 | it->second.data = -1; 61 | } 62 | } 63 | 64 | void DeBuger::startTimer(const string& name) 65 | { 66 | auto t = timers_.find(name); 67 | if(t == timers_.end()) { 68 | printf("Timer = %s\n", name.c_str()); 69 | throw std::runtime_error("startTimer: Timer not registered"); 70 | } 71 | t->second.start(); 72 | } 73 | 74 | void DeBuger::stopTimer(const string& name) 75 | { 76 | auto t = timers_.find(name); 77 | if(t == timers_.end()) { 78 | printf("Timer = %s\n", name.c_str()); 79 | throw std::runtime_error("stopTimer: Timer not registered"); 80 | } 81 | t->second.stop(); 82 | } 83 | 84 | double DeBuger::getTime(const string& name) const 85 | { 86 | auto t = timers_.find(name); 87 | if(t == timers_.end()) { 88 | printf("Timer = %s\n", name.c_str()); 89 | throw std::runtime_error("Timer not registered"); 90 | } 91 | return t->second.getTime(); 92 | } 93 | 94 | void DeBuger::log(const string& name, double data) 95 | { 96 | auto l = logs_.find(name); 97 | if(l == logs_.end()) { 98 | printf("Logger = %s\n", name.c_str()); 99 | throw std::runtime_error("Logger not registered"); 100 | } 101 | l->second.data = data; 102 | l->second.set = true; 103 | } 104 | 105 | void DeBuger::trace() 106 | { 107 | char buffer[128]; 108 | bool first_value = true; 109 | if(!ofs_.is_open()) 110 | throw std::runtime_error("Performance monitor not correctly initialized"); 111 | ofs_.precision(15); 112 | ofs_.setf(std::ios::fixed, std::ios::floatfield ); 113 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 114 | { 115 | if(first_value) { 116 | ofs_ << it->second.getTime(); 117 | first_value = false; 118 | } 119 | else 120 | ofs_ << "," << it->second.getTime(); 121 | } 122 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 123 | { 124 | if(first_value) { 125 | ofs_ << it->second.data; 126 | first_value = false; 127 | } 128 | else 129 | ofs_ << "," << it->second.data; 130 | } 131 | ofs_ << "\n"; 132 | } 133 | 134 | void DeBuger::traceHeader() 135 | { 136 | if(!ofs_.is_open()) 137 | throw std::runtime_error("Performance monitor not correctly initialized"); 138 | bool first_value = true; 139 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 140 | { 141 | if(first_value) { 142 | ofs_ << it->first; 143 | first_value = false; 144 | } 145 | else 146 | ofs_ << "," << it->first; 147 | } 148 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 149 | { 150 | if(first_value) { 151 | ofs_ << it->first; 152 | first_value = false; 153 | } 154 | else 155 | ofs_ << "," << it->first; 156 | } 157 | ofs_ << "\n"; 158 | } 159 | 160 | } // namespace vk 161 | -------------------------------------------------------------------------------- /src/fast_dect.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/core/core.hpp" 2 | #include "opencv2/imgproc/imgproc.hpp" 3 | #include "opencv2/highgui/highgui.hpp" 4 | 5 | #include "opencv2/features2d/features2d.hpp" 6 | #include "opencv2/nonfree/nonfree.hpp" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | using namespace cv; 18 | using namespace std; 19 | 20 | int main() 21 | { 22 | cv::Mat frame, gray; 23 | frame = cv::imread("/home/hyj/bagfiles/1/frame0000.jpg"); 24 | cv::cvtColor(frame,gray, CV_BGR2GRAY); 25 | std::vector< cv::KeyPoint> kpts; 26 | cv::FastFeatureDetector fast(60); 27 | //fast.create("Grid"); 28 | fast.detect(gray,kpts); 29 | drawKeypoints(gray,kpts,gray,cv::Scalar(255)); 30 | 31 | // grid 32 | //cv::GridAdaptedFeatureDetector grid_fast(fast,200,20,20); 33 | //grid_fast.detect(gray,kpts); 34 | // drawKeypoints(frame,kpts,gray,cv::Scalar(255)); 35 | imshow("frame",gray); 36 | 37 | cv::waitKey(0); 38 | return 0; 39 | 40 | } 41 | -------------------------------------------------------------------------------- /src/fast_nonmax_3x3.cpp: -------------------------------------------------------------------------------- 1 | // This function is taken from libCVD. The only change is the return type which 2 | // returns the index of the maxima scores and not a new vector of scores! 3 | // Created on: Dec 18, 2012 4 | // Author: cforster 5 | 6 | #include 7 | #include 8 | 9 | namespace fast 10 | { 11 | 12 | //This function has been moved from fast_corner.cxx. Look there 13 | //for the old ChangeLog. 14 | // fast_nonmax_t is templated so you can have either of: 15 | // 1: A vector of ImageRefs of the nonmax corners 16 | // 2: A vector of pairs of the corners and their scores. 17 | void fast_nonmax_3x3(const std::vector& corners, const std::vector& scores, std::vector& nonmax_corners) 18 | { 19 | nonmax_corners.clear(); 20 | nonmax_corners.reserve(corners.size()); 21 | 22 | if(corners.size() < 1) 23 | return; 24 | 25 | 26 | // Find where each row begins 27 | // (the corners are output in raster scan order). A beginning of -1 signifies 28 | // that there are no corners on that row. 29 | int last_row = corners.back().y; 30 | vector row_start(last_row + 1, -1); 31 | 32 | int prev_row = -1; 33 | for(unsigned int i=0; i< corners.size(); i++) 34 | if(corners[i].y != prev_row) 35 | { 36 | row_start[corners[i].y] = i; 37 | prev_row = corners[i].y; 38 | } 39 | 40 | 41 | //Point above points (roughly) to the pixel above the one of interest, if there 42 | //is a feature there. 43 | int point_above = 0; 44 | int point_below = 0; 45 | 46 | const int sz = (int)corners.size(); 47 | 48 | for(int i=0; i < sz; i++) 49 | { 50 | int score = scores[i]; 51 | fast_xy pos = corners[i]; 52 | 53 | //Check left 54 | if(i > 0) 55 | //if(corners[i-1] == pos-ImageRef(1,0) && (scores[i-1] >= score)) 56 | if(corners[i-1].x == pos.x-1 && corners[i-1].y == pos.y && scores[i-1] >= score) 57 | continue; 58 | 59 | //Check right 60 | if(i < (sz - 1)) 61 | //if(corners[i+1] == pos+ImageRef(1,0) && (scores[i+1] >= score)) 62 | if(corners[i+1].x == pos.x+1 && corners[i+1].y == pos.y && scores[i+1] >= score) 63 | continue; 64 | 65 | //Check above (if there is a valid row above) 66 | if(pos.y != 0 && row_start[pos.y - 1] != -1) 67 | { 68 | //Make sure that current point_above is one 69 | //row above. 70 | if(corners[point_above].y < pos.y - 1) 71 | point_above = row_start[pos.y-1]; 72 | 73 | //Make point_above point to the first of the pixels above the current point, 74 | //if it exists. 75 | for(; corners[point_above].y < pos.y && corners[point_above].x < pos.x - 1; point_above++) 76 | {} 77 | 78 | 79 | for(int i=point_above; corners[i].y < pos.y && corners[i].x <= pos.x + 1; i++) 80 | { 81 | int x = corners[i].x; 82 | if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && (scores[i] >= score)) 83 | goto cont; 84 | } 85 | 86 | } 87 | 88 | //Check below (if there is anything below) 89 | if(pos.y != last_row && row_start[pos.y + 1] != -1 && point_below < sz) //Nothing below 90 | { 91 | if(corners[point_below].y < pos.y + 1) 92 | point_below = row_start[pos.y+1]; 93 | 94 | // Make point below point to one of the pixels belowthe current point, if it 95 | // exists. 96 | for(; point_below < sz && corners[point_below].y == pos.y+1 && corners[point_below].x < pos.x - 1; point_below++) 97 | {} 98 | 99 | for(int i=point_below; i < sz && corners[i].y == pos.y+1 && corners[i].x <= pos.x + 1; i++) 100 | { 101 | int x = corners[i].x; 102 | if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && (scores[i] >= score)) 103 | goto cont; 104 | } 105 | } 106 | 107 | nonmax_corners.push_back(i); 108 | 109 | cont: 110 | ; 111 | } 112 | } 113 | 114 | } 115 | -------------------------------------------------------------------------------- /src/frame_handler_base.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | //#include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace svo 30 | { 31 | 32 | // definition of global and static variables which were declared in the header 33 | #ifdef SVO_TRACE 34 | svo::DeBuger* g_permon = NULL; 35 | #endif 36 | 37 | FrameHandlerBase::FrameHandlerBase() : 38 | stage_(STAGE_PAUSED), 39 | set_reset_(false), 40 | set_start_(false), 41 | acc_frame_timings_(10), 42 | acc_num_obs_(10), 43 | num_obs_last_(0), 44 | tracking_quality_(TRACKING_INSUFFICIENT) 45 | { 46 | #ifdef SVO_TRACE 47 | // Initialize Performance Monitor 48 | g_permon = new svo::DeBuger(); 49 | g_permon->addTimer("pyramid_creation"); 50 | g_permon->addTimer("sparse_img_align"); 51 | g_permon->addTimer("reproject"); 52 | g_permon->addTimer("reproject_kfs"); 53 | g_permon->addTimer("reproject_candidates"); 54 | g_permon->addTimer("feature_align"); 55 | g_permon->addTimer("pose_optimizer"); 56 | g_permon->addTimer("point_optimizer"); 57 | g_permon->addTimer("local_ba"); 58 | g_permon->addTimer("tot_time"); 59 | g_permon->addLog("timestamp"); 60 | g_permon->addLog("img_align_n_tracked"); 61 | g_permon->addLog("repr_n_mps"); 62 | g_permon->addLog("repr_n_new_references"); 63 | g_permon->addLog("sfba_thresh"); 64 | g_permon->addLog("sfba_error_init"); 65 | g_permon->addLog("sfba_error_final"); 66 | g_permon->addLog("sfba_n_edges_final"); 67 | g_permon->addLog("loba_n_erredges_init"); 68 | g_permon->addLog("loba_n_erredges_fin"); 69 | g_permon->addLog("loba_err_init"); 70 | g_permon->addLog("loba_err_fin"); 71 | g_permon->addLog("n_candidates"); 72 | g_permon->addLog("dropout"); 73 | g_permon->init(Config::traceName(), Config::traceDir()); 74 | #endif 75 | 76 | SVO_INFO_STREAM("SVO initialized"); 77 | } 78 | 79 | FrameHandlerBase::~FrameHandlerBase() 80 | { 81 | SVO_INFO_STREAM("SVO destructor invoked"); 82 | #ifdef SVO_TRACE 83 | delete g_permon; 84 | #endif 85 | } 86 | 87 | bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp) 88 | { 89 | if(set_start_) 90 | { 91 | resetAll(); 92 | stage_ = STAGE_FIRST_FRAME; 93 | } 94 | 95 | if(stage_ == STAGE_PAUSED) 96 | return false; 97 | 98 | SVO_LOG(timestamp); 99 | SVO_DEBUG_STREAM("New Frame"); 100 | SVO_START_TIMER("tot_time"); 101 | timer_.start(); 102 | 103 | // some cleanup from last iteration, can't do before because of visualization 104 | map_.emptyTrash(); 105 | return true; 106 | } 107 | 108 | int FrameHandlerBase::finishFrameProcessingCommon( 109 | const size_t update_id, 110 | const UpdateResult dropout, 111 | const size_t num_observations) 112 | { 113 | SVO_DEBUG_STREAM("Frame: "<writeToFile(); 125 | { 126 | boost::unique_lock lock(map_.point_candidates_.mut_); 127 | size_t n_candidates = map_.point_candidates_.candidates_.size(); 128 | SVO_LOG(n_candidates); 129 | } 130 | #endif 131 | 132 | if(dropout == RESULT_FAILURE && 133 | (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) 134 | { 135 | stage_ = STAGE_RELOCALIZING; 136 | tracking_quality_ = TRACKING_INSUFFICIENT; 137 | } 138 | else if (dropout == RESULT_FAILURE) 139 | resetAll(); 140 | if(set_reset_) 141 | resetAll(); 142 | 143 | return 0; 144 | } 145 | 146 | void FrameHandlerBase::resetCommon() 147 | { 148 | map_.reset(); 149 | stage_ = STAGE_PAUSED; 150 | set_reset_ = false; 151 | set_start_ = false; 152 | tracking_quality_ = TRACKING_INSUFFICIENT; 153 | num_obs_last_ = 0; 154 | SVO_INFO_STREAM("RESET"); 155 | } 156 | 157 | void FrameHandlerBase::setTrackingQuality(const size_t num_observations,const size_t init_match_number) 158 | { 159 | tracking_quality_ = TRACKING_GOOD; 160 | if(num_observations < Config::qualityMinFts()) 161 | { 162 | SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!"); 163 | tracking_quality_ = TRACKING_INSUFFICIENT; 164 | } 165 | 166 | //const int feature_drop = static_cast(std::min(num_obs_last_, Config::maxFts())) - num_observations; 167 | //if(feature_drop > Config::qualityMaxFtsDrop()) 168 | const float feature_drop = (float)(init_match_number - num_observations); 169 | if(feature_drop/init_match_number > 0.6) 170 | { 171 | //SVO_WARN_STREAM("outliter during pose "<< feature_drop <<" features!"); 172 | SVO_WARN_STREAM("outliter: "<< feature_drop<<" init_match_number: "<last_structure_optim_ < rhs->last_structure_optim_); 180 | } 181 | 182 | void FrameHandlerBase::optimizeStructure( 183 | FramePtr frame, 184 | size_t max_n_pts, 185 | int max_iter) 186 | { 187 | deque pts; 188 | for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) 189 | { 190 | if((*it)->point != NULL) 191 | pts.push_back((*it)->point); 192 | } 193 | max_n_pts = min(max_n_pts, pts.size()); 194 | nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator); 195 | for(deque::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it) 196 | { 197 | (*it)->optimize(max_iter); 198 | (*it)->last_structure_optim_ = frame->id_; 199 | } 200 | } 201 | 202 | 203 | } // namespace svo 204 | -------------------------------------------------------------------------------- /src/math_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * math_utils.cpp 3 | * 4 | * Created on: Jul 20, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #include 9 | 10 | namespace svo { 11 | 12 | using namespace Eigen; 13 | 14 | Vector3d 15 | triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, 16 | const Vector3d& feature1, const Vector3d& feature2 ) 17 | { 18 | Vector3d f2 = R * feature2; 19 | Vector2d b; 20 | b[0] = t.dot(feature1); 21 | b[1] = t.dot(f2); 22 | Matrix2d A; 23 | A(0,0) = feature1.dot(feature1); 24 | A(1,0) = feature1.dot(f2); 25 | A(0,1) = -A(1,0); 26 | A(1,1) = -f2.dot(f2); 27 | Vector2d lambda = A.inverse() * b; 28 | Vector3d xm = lambda[0] * feature1; 29 | Vector3d xn = t + lambda[1] * f2; 30 | return ( xm + xn )/2; 31 | } 32 | 33 | bool 34 | depthFromTriangulationExact( 35 | const Matrix3d& R_r_c, 36 | const Vector3d& t_r_c, 37 | const Vector3d& f_r, 38 | const Vector3d& f_c, 39 | double& depth_in_r, 40 | double& depth_in_c) 41 | { 42 | // bearing vectors (f_r, f_c) do not need to be unit length 43 | const Vector3d f_c_in_r(R_r_c*f_c); 44 | const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); 45 | const double b = f_c_in_r.dot(t_r_c); 46 | const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); 47 | 48 | if(abs(denom) < 0.000001) 49 | return false; 50 | 51 | depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; 52 | depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); 53 | return true; 54 | } 55 | 56 | double 57 | reprojError(const Vector3d& f1, 58 | const Vector3d& f2, 59 | double error_multiplier2) 60 | { 61 | Vector2d e = project2d(f1) - project2d(f2); 62 | return error_multiplier2 * e.norm(); 63 | } 64 | 65 | double 66 | computeInliers(const vector& features1, // c1 67 | const vector& features2, // c2 68 | const Matrix3d& R, // R_c1_c2 69 | const Vector3d& t, // c1_t 70 | const double reproj_thresh, 71 | double error_multiplier2, 72 | vector& xyz_vec, // in frame c1 73 | vector& inliers, 74 | vector& outliers) 75 | { 76 | inliers.clear(); inliers.reserve(features1.size()); 77 | outliers.clear(); outliers.reserve(features1.size()); 78 | xyz_vec.clear(); xyz_vec.reserve(features1.size()); 79 | double tot_error = 0; 80 | //triangulate all features and compute reprojection errors and inliers 81 | for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) 91 | outliers.push_back(j); 92 | else 93 | { 94 | inliers.push_back(j); 95 | tot_error += e1+e2; 96 | } 97 | } 98 | return tot_error; 99 | } 100 | 101 | void 102 | computeInliersOneView(const vector & feature_sphere_vec, 103 | const vector & xyz_vec, 104 | const Matrix3d &R, 105 | const Vector3d &t, 106 | const double reproj_thresh, 107 | const double error_multiplier2, 108 | vector& inliers, 109 | vector& outliers) 110 | { 111 | inliers.clear(); inliers.reserve(xyz_vec.size()); 112 | outliers.clear(); outliers.reserve(xyz_vec.size()); 113 | for(size_t j = 0; j < xyz_vec.size(); j++ ) 114 | { 115 | double e = reprojError(feature_sphere_vec[j], 116 | R.transpose() * ( xyz_vec[j] - t ), 117 | error_multiplier2); 118 | if(e < reproj_thresh) 119 | inliers.push_back(j); 120 | else 121 | outliers.push_back(j); 122 | } 123 | } 124 | 125 | Vector3d 126 | dcm2rpy(const Matrix3d &R) 127 | { 128 | Vector3d rpy; 129 | rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); 130 | if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) 131 | { 132 | rpy[2] = 0; 133 | rpy[0] = -atan2( R(0,1), R(1,1) ); 134 | } 135 | else 136 | { 137 | if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) 138 | { 139 | rpy[2] = 0; 140 | rpy[0] = -atan2( R(0,1), R(1,1) ); 141 | } 142 | else 143 | { 144 | rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); 145 | rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); 146 | } 147 | } 148 | return rpy; 149 | } 150 | 151 | Matrix3d 152 | rpy2dcm(const Vector3d &rpy) 153 | { 154 | Matrix3d R1; 155 | R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; 156 | R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); 157 | R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); 158 | 159 | Matrix3d R2; 160 | R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); 161 | R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; 162 | R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); 163 | 164 | Matrix3d R3; 165 | R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; 166 | R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; 167 | R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; 168 | 169 | return R3 * R2 * R1; 170 | } 171 | 172 | Quaterniond 173 | angax2quat(const Vector3d& n, const double& angle) 174 | { 175 | // n must be normalized! 176 | double s(sin(angle/2)); 177 | return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); 178 | } 179 | 180 | 181 | Matrix3d 182 | angax2dcm(const Vector3d& n, const double& angle) 183 | { 184 | // n must be normalized 185 | Matrix3d sqewn(sqew(n)); 186 | return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); 187 | } 188 | 189 | double 190 | sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) 191 | { 192 | Vector3d v3Dash = unproject2d(v2Dash); 193 | Vector3d v3 = unproject2d(v2); 194 | 195 | double dError = v3Dash.transpose() * Essential * v3; 196 | 197 | Vector3d fv3 = Essential * v3; 198 | Vector3d fTv3Dash = Essential.transpose() * v3Dash; 199 | 200 | Vector2d fv3Slice = fv3.head<2>(); 201 | Vector2d fTv3DashSlice = fTv3Dash.head<2>(); 202 | 203 | return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); 204 | } 205 | 206 | } // end namespace vk 207 | -------------------------------------------------------------------------------- /src/point.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace svo { 24 | 25 | int Point::point_counter_ = 0; 26 | 27 | Point::Point(const Vector3d& pos) : 28 | id_(point_counter_++), 29 | pos_(pos), 30 | normal_set_(false), 31 | n_obs_(0), 32 | v_pt_(NULL), 33 | last_published_ts_(0), 34 | last_projected_kf_id_(-1), 35 | type_(TYPE_UNKNOWN), 36 | n_failed_reproj_(0), 37 | n_succeeded_reproj_(0), 38 | last_structure_optim_(0) 39 | {} 40 | 41 | Point::Point(const Vector3d& pos, Feature* ftr) : 42 | id_(point_counter_++), 43 | pos_(pos), 44 | normal_set_(false), 45 | n_obs_(1), 46 | v_pt_(NULL), 47 | last_published_ts_(0), 48 | last_projected_kf_id_(-1), 49 | type_(TYPE_UNKNOWN), 50 | n_failed_reproj_(0), 51 | n_succeeded_reproj_(0), 52 | last_structure_optim_(0) 53 | { 54 | obs_.push_front(ftr); 55 | } 56 | 57 | Point::~Point() 58 | {} 59 | 60 | void Point::addFrameRef(Feature* ftr) 61 | { 62 | obs_.push_front(ftr); 63 | ++n_obs_; 64 | } 65 | 66 | Feature* Point::findFrameRef(Frame* frame) 67 | { 68 | for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) 69 | if((*it)->frame == frame) 70 | return *it; 71 | return NULL; // no keyframe found 72 | } 73 | 74 | bool Point::deleteFrameRef(Frame* frame) 75 | { 76 | for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) 77 | { 78 | if((*it)->frame == frame) 79 | { 80 | obs_.erase(it); 81 | return true; 82 | } 83 | } 84 | return false; 85 | } 86 | 87 | void Point::initNormal() 88 | { 89 | assert(!obs_.empty()); 90 | const Feature* ftr = obs_.back(); 91 | assert(ftr->frame != NULL); 92 | normal_ = ftr->frame->T_f_w_.rotationMatrix().transpose()*(-ftr->f); 93 | normal_information_ = DiagonalMatrix(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0); 94 | normal_set_ = true; 95 | } 96 | 97 | bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const 98 | { 99 | // TODO: get frame with same point of view AND same pyramid level! 100 | Vector3d obs_dir(framepos - pos_); obs_dir.normalize(); 101 | auto min_it=obs_.begin(); 102 | double min_cos_angle = 0; 103 | for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) 104 | { 105 | Vector3d dir((*it)->frame->pos() - pos_); dir.normalize(); 106 | double cos_angle = obs_dir.dot(dir); 107 | if(cos_angle > min_cos_angle) 108 | { 109 | min_cos_angle = cos_angle; 110 | min_it = it; 111 | } 112 | } 113 | ftr = *min_it; 114 | if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless 115 | return false; 116 | return true; 117 | } 118 | /* 119 | void Point::optimize(const size_t n_iter) 120 | { 121 | Vector3d old_point = pos_; 122 | double chi2 = 0.0; 123 | Matrix3d A; 124 | Vector3d b; 125 | 126 | for(size_t i=0; iframe->T_f_w_ * pos_); 137 | Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J); 138 | const Vector2d e(project2d((*it)->f) - project2d(p_in_f)); 139 | new_chi2 += e.squaredNorm(); 140 | A.noalias() += J.transpose() * J; 141 | b.noalias() -= J.transpose() * e; 142 | } 143 | 144 | // solve linear system 145 | const Vector3d dp(A.ldlt().solve(b)); 146 | 147 | // check if error increased 148 | if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) 149 | { 150 | #ifdef POINT_OPTIMIZER_DEBUG 151 | cout << "it " << i 152 | << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; 153 | #endif 154 | pos_ = old_point; // roll-back 155 | break; 156 | } 157 | 158 | // update the model 159 | Vector3d new_point = pos_ + dp; 160 | old_point = pos_; 161 | pos_ = new_point; 162 | chi2 = new_chi2; 163 | 164 | // stop when converged 165 | if(norm_max(dp) <= EPS) 166 | break; 167 | } 168 | #ifdef POINT_OPTIMIZER_DEBUG 169 | cout << endl; 170 | #endif 171 | } 172 | 173 | */ 174 | void Point::optimize(const size_t n_iter) 175 | { 176 | Vector3d old_point = pos_; 177 | double chi2 = 0.0; 178 | Matrix3d A; 179 | Vector3d b; 180 | 181 | for(size_t i=0; iframe->T_f_w_ * pos_); 192 | Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J); 193 | const Vector2d e(project2d((*it)->f) - project2d(p_in_f)); 194 | 195 | if((*it)->type == Feature::EDGELET) 196 | { 197 | float err_edge = (*it)->grad.transpose() * e; 198 | new_chi2 += err_edge * err_edge; 199 | A.noalias() += J.transpose()*(*it)->grad * (*it)->grad.transpose() * J; 200 | b.noalias() -= J.transpose() *(*it)->grad * err_edge; 201 | } 202 | else 203 | { 204 | new_chi2 += e.squaredNorm(); 205 | A.noalias() += J.transpose() * J; 206 | b.noalias() -= J.transpose() * e; 207 | } 208 | 209 | } 210 | 211 | // solve linear system 212 | const Vector3d dp(A.ldlt().solve(b)); 213 | 214 | // check if error increased 215 | if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) 216 | { 217 | #ifdef POINT_OPTIMIZER_DEBUG 218 | cout << "it " << i 219 | << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; 220 | #endif 221 | pos_ = old_point; // roll-back 222 | break; 223 | } 224 | 225 | // update the model 226 | Vector3d new_point = pos_ + dp; 227 | old_point = pos_; 228 | pos_ = new_point; 229 | chi2 = new_chi2; 230 | 231 | // stop when converged 232 | if(norm_max(dp) <= EPS) 233 | break; 234 | } 235 | #ifdef POINT_OPTIMIZER_DEBUG 236 | cout << endl; 237 | #endif 238 | } 239 | 240 | 241 | } // namespace svo 242 | -------------------------------------------------------------------------------- /src/robust_cost.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of dvo. 3 | * 4 | * Copyright 2012 Christian Kerl (Technical University of Munich) 5 | * For more information see . 6 | * 7 | * dvo 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 | * dvo 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 dvo. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace svo { 27 | namespace robust_cost { 28 | 29 | const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; 30 | const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; 31 | 32 | TDistributionScaleEstimator:: 33 | TDistributionScaleEstimator(const float dof) : 34 | dof_(dof), 35 | initial_sigma_(INITIAL_SIGMA) 36 | {} 37 | 38 | float TDistributionScaleEstimator:: 39 | compute(std::vector& errors) const 40 | { 41 | float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); 42 | int num = 0; 43 | float lambda = initial_lamda; 44 | int iterations = 0; 45 | do 46 | { 47 | ++iterations; 48 | initial_lamda = lambda; 49 | num = 0; 50 | lambda = 0.0f; 51 | 52 | for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) 53 | { 54 | if(std::isfinite(*it)) 55 | { 56 | ++num; 57 | const float error2 = (*it)*(*it); 58 | lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); 59 | } 60 | } 61 | lambda = float(num) / lambda; 62 | } while(std::abs(lambda - initial_lamda) > 1e-3); 63 | 64 | return std::sqrt(1.0f / lambda); 65 | } 66 | 67 | const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 68 | 69 | float MADScaleEstimator:: 70 | compute(std::vector& errors) const 71 | { 72 | // error must be in absolute values! 73 | return NORMALIZER * svo::getMedian(errors); 74 | } 75 | 76 | float NormalDistributionScaleEstimator:: 77 | compute(std::vector& errors) const 78 | { 79 | const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); 80 | float var = 0.0; 81 | std::for_each(errors.begin(), errors.end(), [&](const float d) { 82 | var += (d - mean) * (d - mean); 83 | }); 84 | return std::sqrt(var); // return standard deviation 85 | } 86 | 87 | const float TukeyWeightFunction::DEFAULT_B = 4.6851f; 88 | 89 | TukeyWeightFunction::TukeyWeightFunction(const float b) 90 | { 91 | configure(b); 92 | } 93 | 94 | float TukeyWeightFunction::value(const float& x) const 95 | { 96 | const float x_square = x * x; 97 | if(x_square <= b_square) 98 | { 99 | const float tmp = 1.0f - x_square / b_square; 100 | return tmp * tmp; 101 | } 102 | else 103 | { 104 | return 0.0f; 105 | } 106 | } 107 | 108 | void TukeyWeightFunction:: 109 | configure(const float& param) 110 | { 111 | b_square = param * param; 112 | } 113 | 114 | const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; 115 | 116 | TDistributionWeightFunction:: 117 | TDistributionWeightFunction(const float dof) 118 | { 119 | configure(dof); 120 | } 121 | 122 | float TDistributionWeightFunction:: 123 | value(const float & x) const 124 | { 125 | return ((dof_ + 1.0f) / (dof_ + (x * x))); 126 | } 127 | 128 | void TDistributionWeightFunction:: 129 | configure(const float& param) 130 | { 131 | dof_ = param; 132 | normalizer_ = dof_ / (dof_ + 1.0f); 133 | } 134 | 135 | const float HuberWeightFunction::DEFAULT_K = 1.345f; 136 | 137 | HuberWeightFunction:: 138 | HuberWeightFunction(const float k) 139 | { 140 | configure(k); 141 | } 142 | 143 | void HuberWeightFunction:: 144 | configure(const float& param) 145 | { 146 | k = param; 147 | } 148 | 149 | float HuberWeightFunction:: 150 | value(const float& t) const 151 | { 152 | const float t_abs = std::abs(t); 153 | if(t_abs < k) 154 | return 1.0f; 155 | else 156 | return k / t_abs; 157 | } 158 | 159 | } // namespace robust_cost 160 | } // namespace vk 161 | 162 | 163 | -------------------------------------------------------------------------------- /test/test_live_vo.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | class BenchmarkNode 33 | { 34 | svo::AbstractCamera* cam_; 35 | svo::PinholeCamera* cam_pinhole_; 36 | svo::FrameHandlerMono* vo_; 37 | 38 | SLAM_VIEWER::Viewer* viewer_; 39 | std::thread * viewer_thread_; 40 | 41 | public: 42 | BenchmarkNode(); 43 | ~BenchmarkNode(); 44 | 45 | void InitSystem(int h, int w, double fov); 46 | void runFromFolder(); 47 | }; 48 | 49 | BenchmarkNode::BenchmarkNode() 50 | { 51 | /* 52 | cam_pinhole_ = new svo::PinholeCamera(640,480,502.961104,503.651566, 284.978460, 247.527333, 53 | -0.378740,0.133422, -0.001505, -0.001445); 54 | cam_ = new svo::PinholeCamera(640,480,407.763641, 453.693298, 267.111836,247.958895); 55 | */ 56 | // cam_ = new svo::PinholeCamera(640,480,482.62565,480.83271, 323.96419, 261.20336, -0.031563,0.165711,0.001507,-0.00083,-0.18942); 57 | // cam_ = new svo::PinholeCamera(368, 640, 450., 450., 184., 320., 0.0, 0.0, 0.0, 0.0, 0.0); 58 | // cam_ = new svo::PinholeCamera(640, 368, 450., 450., 320., 184., 0.0, 0.0, 0.0, 0.0, 0.0); 59 | // // cam_ = new svo::PinholeCamera(960, 544, 450., 450., 960./2., 544/2., 0.0, 0.0, 0.0, 0.0, 0.0); 60 | 61 | // vo_ = new svo::FrameHandlerMono(cam_); 62 | // vo_->start(); 63 | 64 | // viewer_ = new SLAM_VIEWER::Viewer(vo_); 65 | // viewer_thread_ = new std::thread(&SLAM_VIEWER::Viewer::run,viewer_); 66 | // viewer_thread_->detach(); 67 | 68 | } 69 | 70 | void BenchmarkNode::InitSystem(int w, int h, double fov) 71 | { 72 | double focal = w / std::tan(fov/2.); 73 | std::cout << " focal = " << focal << std::endl; 74 | 75 | cam_ = new svo::PinholeCamera(w, h, focal, focal, w/2., h/2.); 76 | 77 | vo_ = new svo::FrameHandlerMono(cam_); 78 | vo_->start(); 79 | 80 | viewer_ = new SLAM_VIEWER::Viewer(vo_); 81 | viewer_thread_ = new std::thread(&SLAM_VIEWER::Viewer::run,viewer_); 82 | viewer_thread_->detach(); 83 | } 84 | 85 | BenchmarkNode::~BenchmarkNode() 86 | { 87 | delete vo_; 88 | delete cam_; 89 | delete cam_pinhole_; 90 | 91 | delete viewer_; 92 | delete viewer_thread_; 93 | } 94 | 95 | //#define TXTREAD 96 | void BenchmarkNode::runFromFolder() 97 | { 98 | 99 | // cv::VideoCapture cap(1); // open the default camera 100 | cv::VideoCapture cap("/home/heyijia/testdata/15.mp4"); // open the default camera 101 | 102 | if (!cap.isOpened()) // check if we succeeded 103 | return ; 104 | 105 | int img_id = 0; 106 | bool init_flag = true; 107 | for (;;) { 108 | 109 | cv::Mat image; 110 | cap.read(image); // get a new frame from camera 111 | 112 | // std::cout <<" image size " << image.size() << std::endl; 113 | 114 | assert(!image.empty()); 115 | img_id++; 116 | 117 | if( image.cols > 1000 || image.rows > 1000) 118 | { 119 | cv::resize(image, image, image.size()/2); 120 | 121 | } 122 | if(init_flag) 123 | { 124 | init_flag = false; 125 | int w = image.cols; 126 | int h = image.rows; 127 | InitSystem(w, h, 90 / 57.3); 128 | } 129 | // if(img_id < 800) continue; 130 | 131 | // cv::imshow("origin_image", image); 132 | // if (cv::waitKey(1) >= 0) break; 133 | 134 | cv::Mat gray; 135 | cv::cvtColor(image,gray,CV_BGR2GRAY); 136 | 137 | /* 138 | cv::Mat unimg; 139 | cam_pinhole_->undistortImage(gray,unimg); 140 | vo_->addImage(unimg, 0.01*img_id); 141 | */ 142 | vo_->addImage(gray, 0.01*img_id); 143 | 144 | // display tracking quality 145 | if(vo_->lastFrame() != NULL) 146 | { 147 | std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" 148 | << "#Features: " << vo_->lastNumObservations() << " \n"; 149 | //<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n"; 150 | // std::cout<<"Frame pose: "<< vo_->lastFrame()->T_f_w_ <lastFrame()->w2c(Rwl * Eigen::Vector3d(0, 0, 0) + twl); 161 | Eigen::Vector2d x = vo_->lastFrame()->w2c(Rwl * Eigen::Vector3d(axis_len, 0, 0) + twl); 162 | Eigen::Vector2d y = vo_->lastFrame()->w2c(Rwl * Eigen::Vector3d(0, -axis_len, 0) + twl); 163 | Eigen::Vector2d z = vo_->lastFrame()->w2c(Rwl * Eigen::Vector3d(0, 0, -axis_len) + twl); 164 | 165 | cv::line(image, cv::Point2f(o.x(), o.y()),cv::Point2f(x.x(), x.y()), cv::Scalar(255,0,0), 2); 166 | cv::line(image, cv::Point2f(o.x(), o.y()),cv::Point2f(y.x(), y.y()), cv::Scalar(0,255,0), 2); 167 | cv::line(image, cv::Point2f(o.x(), o.y()),cv::Point2f(z.x(), z.y()), cv::Scalar(0,0,255), 2); 168 | 169 | cv::imshow("origin_image", image); 170 | cv::waitKey(0); 171 | } 172 | 173 | } 174 | 175 | cap.release(); 176 | return; 177 | 178 | } 179 | 180 | 181 | int main(int argc, char** argv) 182 | { 183 | 184 | 185 | BenchmarkNode benchmark; 186 | benchmark.runFromFolder(); 187 | 188 | printf("BenchmarkNode finished.\n"); 189 | return 0; 190 | } 191 | 192 | -------------------------------------------------------------------------------- /test/test_pipel_euroc.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace svo { 33 | 34 | void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes, 35 | vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps) 36 | { 37 | ifstream fTimes; 38 | fTimes.open(strPathTimes.c_str()); 39 | vTimeStamps.reserve(5000); 40 | vstrImageLeft.reserve(5000); 41 | vstrImageRight.reserve(5000); 42 | while(!fTimes.eof()) 43 | { 44 | string s; 45 | getline(fTimes,s); 46 | if(!s.empty()) 47 | { 48 | stringstream ss; 49 | ss << s; 50 | vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png"); 51 | vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png"); 52 | double t; 53 | ss >> t; 54 | vTimeStamps.push_back(t/1e9); 55 | 56 | } 57 | } 58 | } 59 | 60 | class BenchmarkNode 61 | { 62 | svo::AbstractCamera* cam_; 63 | svo::AbstractCamera* cam_r_; 64 | svo::PinholeCamera* cam_pinhole_; 65 | svo::FrameHandlerMono* vo_; 66 | 67 | SLAM_VIEWER::Viewer* viewer_; 68 | std::thread * viewer_thread_; 69 | 70 | public: 71 | BenchmarkNode(); 72 | ~BenchmarkNode(); 73 | void runFromFolder(); 74 | }; 75 | 76 | BenchmarkNode::BenchmarkNode() 77 | { 78 | 79 | cam_ = new svo::PinholeCamera(752, 480, 435.2046959714599, 435.2046959714599, 367.4517211914062,252.2008514404297); 80 | cam_r_ = new svo::PinholeCamera(752, 480,435.2046959714599, 435.2046959714599, 367.4517211914062, 252.2008514404297); 81 | 82 | vo_ = new svo::FrameHandlerMono(cam_); 83 | vo_->start(); 84 | 85 | viewer_ = new SLAM_VIEWER::Viewer(vo_); 86 | viewer_thread_ = new std::thread(&SLAM_VIEWER::Viewer::run,viewer_); 87 | viewer_thread_->detach(); 88 | 89 | } 90 | 91 | BenchmarkNode::~BenchmarkNode() 92 | { 93 | delete vo_; 94 | delete cam_; 95 | delete cam_r_; 96 | // delete cam_pinhole_; 97 | 98 | delete viewer_; 99 | delete viewer_thread_; 100 | } 101 | 102 | //#define TXTREAD 103 | void BenchmarkNode::runFromFolder() 104 | { 105 | 106 | // Retrieve paths to images 107 | vector vstrImageLeft; 108 | vector vstrImageRight; 109 | vector vTimeStamp; 110 | LoadImages("/media/hyj/dataset/datasets/MH_01_easy/mav0/cam0/data", "/media/hyj/dataset/datasets/MH_01_easy/mav0/cam1/data", "MH01.txt", vstrImageLeft, vstrImageRight, vTimeStamp); 111 | 112 | if(vstrImageLeft.empty() || vstrImageRight.empty()) 113 | { 114 | //cerr << "ERROR: No images in provided path." << endl; 115 | return ; 116 | } 117 | 118 | if(vstrImageLeft.size()!=vstrImageRight.size()) 119 | { 120 | //cerr << "ERROR: Different number of left and right images." << endl; 121 | return ; 122 | } 123 | 124 | // Read rectification parameters 125 | cv::FileStorage fsSettings("EuRoC.yaml", cv::FileStorage::READ); 126 | if(!fsSettings.isOpened()) 127 | { 128 | //cerr << "ERROR: Wrong path to settings" << endl; 129 | return ; 130 | } 131 | 132 | cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; 133 | fsSettings["LEFT.K"] >> K_l; 134 | fsSettings["RIGHT.K"] >> K_r; 135 | 136 | fsSettings["LEFT.P"] >> P_l; 137 | fsSettings["RIGHT.P"] >> P_r; 138 | 139 | fsSettings["LEFT.R"] >> R_l; 140 | fsSettings["RIGHT.R"] >> R_r; 141 | 142 | fsSettings["LEFT.D"] >> D_l; 143 | fsSettings["RIGHT.D"] >> D_r; 144 | 145 | int rows_l = fsSettings["LEFT.height"]; 146 | int cols_l = fsSettings["LEFT.width"]; 147 | int rows_r = fsSettings["RIGHT.height"]; 148 | int cols_r = fsSettings["RIGHT.width"]; 149 | 150 | if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || 151 | rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0) 152 | { 153 | //cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl; 154 | return ; 155 | } 156 | 157 | cv::Mat M1l,M2l,M1r,M2r; 158 | cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); 159 | 160 | const int nImages = vstrImageLeft.size(); 161 | 162 | cv::Mat imLeft, imRight; 163 | for(int ni=100; niaddImage(imLeft_rect, 0.01*ni); 174 | 175 | // display tracking quality 176 | if(vo_->lastFrame() != NULL) 177 | { 178 | std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" 179 | << "#Features: " << vo_->lastNumObservations() << " \n"; 180 | 181 | // access the pose of the camera via vo_->lastFrame()->T_f_w_. 182 | // std::cout<<"Frame pose: "<< vo_->lastFrame()->T_f_w_ < 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace svo { 33 | 34 | void LoadImages(const std::string &strFile, std::vector &vstrImageFilenames, std::vector &vTimestamps) 35 | { 36 | std::ifstream f; 37 | f.open(strFile.c_str()); 38 | 39 | // skip first three lines 40 | std::string s0; 41 | std::getline(f,s0); 42 | std::getline(f,s0); 43 | std::getline(f,s0); 44 | 45 | while(!f.eof()) 46 | { 47 | std::string s; 48 | std::getline(f,s); 49 | if(!s.empty()) 50 | { 51 | std::stringstream ss; 52 | ss << s; 53 | double t; 54 | ss >> t; 55 | vTimestamps.push_back(t); 56 | //ss >> sRGB; 57 | std::string sRGB; 58 | ss >> sRGB; 59 | //sRGB = s + ".jpg"; 60 | //std::cout<start(); 100 | 101 | viewer_ = new SLAM_VIEWER::Viewer(vo_); 102 | viewer_thread_ = new std::thread(&SLAM_VIEWER::Viewer::run,viewer_); 103 | viewer_thread_->detach(); 104 | 105 | } 106 | 107 | BenchmarkNode::~BenchmarkNode() 108 | { 109 | delete vo_; 110 | delete cam_; 111 | // delete cam_pinhole_; 112 | 113 | delete viewer_; 114 | delete viewer_thread_; 115 | } 116 | 117 | #define TXTREAD 118 | void BenchmarkNode::runFromFolder() 119 | { 120 | #ifdef TXTREAD // read image filename with txt, TUM rgbd datasets 121 | std::vector vstrImageFilenames; 122 | std::vector vTimestamps; 123 | std::string filepath = std::string("/media/hyj/dataset/datasets/freiburg2_desk"); 124 | std::string strFile = filepath + "/rgb.txt"; 125 | LoadImages(strFile, vstrImageFilenames, vTimestamps); 126 | 127 | int nImages = vstrImageFilenames.size(); 128 | cv::Mat img; 129 | 130 | for(int ni=0; niundistortImage(img,unimg); 139 | //vo_->addImage(unimg, vTimestamps[ni]); 140 | vo_->addImage(img, vTimestamps[ni]); 141 | 142 | if(vo_->lastFrame() != NULL) 143 | { 144 | 145 | std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" 146 | << "#Features: " << vo_->lastNumObservations() << " \n"; 147 | 148 | } 149 | } 150 | #else // read image filename with id, ICL-NUIM rgbd datasets 151 | for(int img_id =10; img_id < 2700; ++img_id) 152 | { 153 | // load image 154 | std::stringstream ss; 155 | 156 | ss << "/media/hyj/dataset/datasets/living_room_traj3_frei_png/rgb/" << img_id << ".png"; //26_1, 28_6, 11_3 157 | 158 | if(img_id == 1) 159 | std::cout << "reading image " << ss.str() << std::endl; 160 | cv::Mat img(cv::imread(ss.str().c_str(), CV_LOAD_IMAGE_GRAYSCALE)); 161 | 162 | assert(!img.empty()); 163 | 164 | // process frame 165 | //cv::Mat unimg; 166 | //cam_pinhole_->undistortImage(img,unimg); 167 | //vo_->addImage(unimg, 0.01*img_id); 168 | vo_->addImage(img, 0.01*img_id); 169 | 170 | 171 | // display tracking quality 172 | if(vo_->lastFrame() != NULL) 173 | { 174 | std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t" 175 | << "#Features: " << vo_->lastNumObservations() << " \n"; 176 | //<< "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n"; 177 | } 178 | } 179 | #endif 180 | } 181 | 182 | } // namespace svo 183 | 184 | int main(int argc, char** argv) 185 | { 186 | svo::BenchmarkNode benchmark; 187 | benchmark.runFromFolder(); 188 | 189 | return 0; 190 | } 191 | 192 | --------------------------------------------------------------------------------