├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── CMakeLists.txt ├── COPYING.txt ├── LICENSE.txt ├── README.md ├── cmake-modules ├── FindEigen.cmake ├── FindG2O.cmake ├── FindOpenGV.cmake ├── FindSuiteSparse.cmake ├── FindTBB.cmake ├── dependencies.cmake └── options.cmake ├── configurationFiles ├── euroc_gftt_brief.yaml ├── euroc_orb_brief.yaml ├── kitti.yaml ├── kitti_cam_00_to_02_13_to_21.yaml ├── kitti_cam_03.yaml ├── kitti_cam_04_to_12.yaml ├── kitti_gftt_brisk.yaml ├── level7.yaml └── mit.yaml ├── launch ├── euroc.launch ├── kitti.launch ├── level7.launch └── mit.launch ├── package.xml ├── scripts └── euroc_add_camera_info.py ├── sptam_coordinate_systems.png ├── sptam_coordinate_systems_prediction.png ├── sptam_plugin.xml └── src ├── ros ├── CMakeLists.txt ├── base_driver.cpp ├── base_driver.hpp ├── sptam_node.cpp ├── sptam_nodelet.cpp ├── stereo_driver.cpp ├── stereo_driver.hpp └── utils │ ├── opencv_parsers.cpp │ ├── opencv_parsers.hpp │ ├── tf2eigen.hpp │ ├── tf_utils.cpp │ └── tf_utils.hpp ├── sptam ├── BundleDriver.cpp ├── BundleDriver.hpp ├── CMakeLists.txt ├── Camera.cpp ├── Camera.hpp ├── CameraParameters.hpp ├── CameraPose.cpp ├── CameraPose.hpp ├── CovisibilityWindow.cpp ├── CovisibilityWindow.hpp ├── FeatureExtractorThread.cpp ├── FeatureExtractorThread.hpp ├── Frame.cpp ├── Frame.hpp ├── FrustumCulling.cpp ├── FrustumCulling.hpp ├── ILocalWindow.hpp ├── ImageFeatures.cpp ├── ImageFeatures.hpp ├── KeyFramePolicy.hpp ├── MEAS.hpp ├── Map.hpp ├── MapMaker.cpp ├── MapMaker.hpp ├── MapMakerThread.cpp ├── MapMakerThread.hpp ├── MapPoint.cpp ├── MapPoint.hpp ├── Match.hpp ├── Measurement.cpp ├── Measurement.hpp ├── MotionModel.cpp ├── MotionModel.hpp ├── ParallelFeatureExtractor.hpp ├── PosePredictor.hpp ├── RectifiedCameraParameters.hpp ├── RowMatcher.cpp ├── RowMatcher.hpp ├── StereoFrame.cpp ├── StereoFrame.hpp ├── StereoGraph.cpp ├── TrackerViewStereo.cpp ├── TrackerViewStereo.hpp ├── TrackingReport.hpp ├── g2o_driver.cpp ├── g2o_driver.hpp ├── loopclosing │ ├── LCDetector.hpp │ ├── LoopClosing.cpp │ ├── LoopClosing.hpp │ ├── PoseEstimator.cpp │ ├── PoseEstimator.hpp │ ├── SmoothEstimatePropagator.cpp │ ├── SmoothEstimatePropagator.hpp │ ├── StereoMatcher.cpp │ ├── StereoMatcher.hpp │ └── detectors │ │ ├── DLDLoopDetector.cpp │ │ ├── DLDLoopDetector.hpp │ │ ├── FBRISK.cpp │ │ └── FBRISK.h ├── match_to_points.cpp ├── match_to_points.hpp ├── sptam.cpp ├── sptam.hpp ├── sptamParameters.hpp ├── tracker_g2o.cpp ├── tracker_g2o.hpp ├── types_sba_extension.cpp ├── types_sba_extension.hpp └── utils │ ├── CovisibilityGraph.cpp │ ├── CovisibilityGraph.hpp │ ├── Hash2D.hpp │ ├── Iterable.hpp │ ├── concurrent_queue.hpp │ ├── covariance_ellipsoid.hpp │ ├── cv2eigen.hpp │ ├── draw │ ├── Draw.cpp │ ├── Draw.hpp │ └── TrackerView.hpp │ ├── eigen_alignment.hpp │ ├── fixed_queue.hpp │ ├── log │ ├── Logger.cpp │ ├── Logger.hpp │ ├── Profiler.hpp │ └── ScopedProfiler.hpp │ ├── macros.hpp │ ├── pose_covariance.cpp │ ├── pose_covariance.hpp │ ├── projection_derivatives.cpp │ ├── projection_derivatives.hpp │ ├── projective_math.hpp │ ├── set_union.hpp │ ├── time │ └── time.h │ ├── timer.cpp │ └── timer.h ├── standAlone ├── .gitignore ├── CMakeLists.txt ├── FrameGenerator │ ├── FileSequenceFrameGenerator.cpp │ ├── FileSequenceFrameGenerator.h │ ├── FrameGeneratorFactory.hpp │ ├── IFrameGenerator.h │ ├── ListOfFilesFrameGenerator.cpp │ ├── ListOfFilesFrameGenerator.h │ ├── VideoFileFrameGenerator.cpp │ └── VideoFileFrameGenerator.h ├── KITTIGroundTruth.cpp ├── KITTIGroundTruth.hpp ├── SptamWrapper.cpp ├── SptamWrapper.hpp ├── SptamWrapperThread.cpp ├── StereoImageFeatures.hpp ├── Timestamps.cpp ├── Timestamps.hpp ├── configuration_parser_opencv2.hpp ├── configuration_parser_opencv3.hpp ├── sptam-stereo.cpp └── utils │ ├── ProgramOptions.cpp │ └── ProgramOptions.hpp └── tests ├── CMakeLists.txt └── sptam ├── CMakeLists.txt ├── CovisibilityGraph ├── CMakeLists.txt └── test_CovisibilityGraph.cpp └── utils ├── CMakeLists.txt └── set_union ├── CMakeLists.txt └── test_set_union.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # CMake # 2 | ######### 3 | CMakeCache.txt 4 | CMakeFiles 5 | CMakeScripts 6 | Makefile 7 | cmake_install.cmake 8 | install_manifest.txt 9 | CMakeLists.txt.user* 10 | 11 | # Documentation # 12 | ################# 13 | doc/ 14 | 15 | # Compiled source # 16 | ################### 17 | *.com 18 | *.class 19 | *.o 20 | *.slo 21 | *.lo 22 | *.obj 23 | 24 | # Precompiled Headers # 25 | ####################### 26 | *.gch 27 | *.pch 28 | 29 | # Compiled Dynamic libraries # 30 | ############################## 31 | *.so 32 | *.dylib 33 | *.dll 34 | 35 | # Fortran module files # 36 | ######################## 37 | *.mod 38 | 39 | # Compiled Static libraries # 40 | ############################# 41 | *.lai 42 | *.la 43 | *.a 44 | *.lib 45 | 46 | # Executables # 47 | ############### 48 | *.exe 49 | *.out 50 | *.app 51 | 52 | # Packages # 53 | ############ 54 | # it's better to unpack these files and commit the raw source 55 | # git has its own built in compression methods 56 | *.7z 57 | *.dmg 58 | *.gz 59 | *.iso 60 | *.jar 61 | *.rar 62 | *.tar 63 | *.zip 64 | 65 | # Logs and databases # 66 | ###################### 67 | *.log 68 | *.dat 69 | 70 | # Images # 71 | ########## 72 | *.pdf 73 | *.jpg 74 | 75 | # Project files # 76 | ################## 77 | *.prj 78 | .geanyprj 79 | 80 | # Temporal files # 81 | ################## 82 | *.*~ 83 | 84 | # Byte-compiled / optimized / DLL files # 85 | ######################################### 86 | __pycache__/ 87 | *.py[cod] 88 | *$py.class 89 | 90 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | before_script: 2 | - /prepare.sh 3 | - apt-get install -y python-matplotlib 4 | 5 | build: 6 | stage: build 7 | tags: 8 | - ros-kinetic 9 | script: 10 | # Build ROS in four configurations 11 | - /build.sh --cmake-args -DSHOW_TRACKED_FRAMES=ON -DSHOW_PROFILING=ON 12 | - /build.sh --cmake-args -DSHOW_TRACKED_FRAMES=ON -DSHOW_PROFILING=OFF 13 | - /build.sh --cmake-args -DSHOW_TRACKED_FRAMES=OFF -DSHOW_PROFILING=ON 14 | - /build.sh --cmake-args -DSHOW_TRACKED_FRAMES=OFF -DSHOW_PROFILING=OFF 15 | # Build standalone in release config for running tests 16 | - cd $CI_PROJECT_DIR/src/standAlone && mkdir -p build && cd build && 17 | cmake -DSHOW_TRACKED_FRAMES=OFF -DSHOW_PROFILING=ON -DCMAKE_BUILD_TYPE=Release .. && make 18 | # Run KITTI 04 in standalone mode 19 | - cd $CI_PROJECT_DIR/src/standAlone && 20 | ./build/sptam-stereo --timestamps /datasets/KITTI/04/times.txt 21 | ../../configurationFiles/kitti.yaml 22 | ../../configurationFiles/kitti_cam_04_to_12.yaml 23 | /datasets/KITTI/04/image_0 /datasets/KITTI/04/image_1 24 | dir 25 | # Run plot-errors to compute errors and make plots 26 | - $CI_PROJECT_DIR/plotters/plot-errors.py --savetofiles --kitti /datasets/KITTI/poses/04.txt $CI_PROJECT_DIR/src/standAlone/*.log > $CI_PROJECT_DIR/src/standAlone/errors.log 27 | artifacts: 28 | paths: 29 | - $CI_PROJECT_DIR/src/standAlone/*.log 30 | - $CI_PROJECT_DIR/src/standAlone/*.png 31 | expire_in: '1 week' 32 | 33 | 34 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "bow_voc"] 2 | path = bow_voc 3 | url = https://github.com/lrse/bow_vocabularies.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project( sptam ) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake-modules) 5 | 6 | # include macros for program options 7 | include( options ) 8 | 9 | # include finds for common program dependencies 10 | include( dependencies ) 11 | 12 | # Build the sptam library 13 | add_subdirectory( src/sptam ) 14 | 15 | # build unit tests 16 | enable_testing() 17 | add_subdirectory( src/tests ) 18 | 19 | # When building with ROS 20 | if( CATKIN_DEVEL_PREFIX ) 21 | 22 | set(BUILDING_ROS ON) 23 | add_definitions(-DBUILDING_ROS) 24 | 25 | ## Find catkin macros and libraries 26 | find_package(catkin REQUIRED COMPONENTS 27 | roscpp 28 | cmake_modules 29 | std_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | cv_bridge 33 | message_filters 34 | image_geometry 35 | pcl_ros 36 | nav_msgs 37 | tf2 38 | tf2_geometry_msgs 39 | tf2_ros 40 | image_transport 41 | nodelet 42 | ) 43 | include_directories(${catkin_INCLUDE_DIRS}) 44 | 45 | ################################### 46 | ## catkin specific configuration ## 47 | ################################### 48 | ## The catkin_package macro generates cmake config files for your package 49 | ## Declare things to be passed to dependent projects 50 | ## INCLUDE_DIRS: uncomment this if you package contains header files 51 | ## LIBRARIES: libraries you create in this project that dependent projects also need 52 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 53 | ## DEPENDS: system dependencies of this project that dependent projects also need 54 | catkin_package( 55 | CATKIN_DEPENDS roscpp std_msgs sensor_msgs geometry_msgs cv_bridge message_filters image_geometry pcl_ros nav_msgs tf2 tf2_geometry_msgs tf2_ros nodelet 56 | # DEPENDS system_lib 57 | ) 58 | 59 | ########### 60 | ## Build ## 61 | ########### 62 | 63 | # Build the sptam node 64 | add_subdirectory( src/ros ) 65 | 66 | # When building standalone 67 | else() # CATKIN_DEVEL_PREFIX 68 | 69 | add_subdirectory( src/standAlone ) 70 | 71 | endif() # CATKIN_DEVEL_PREFIX 72 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | S-PTAM is released under a GPLv3 license (see COPYING.txt). 2 | 3 | For a closed-source version of S-PTAM for commercial purposes, please contact the authors. 4 | 5 | If you use S-PTAM in an academic work, please cite the most relevant publication associated by visiting: 6 | http://robotica.dc.uba.ar/index.php/publicaciones/?lang=en 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /cmake-modules/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 | -------------------------------------------------------------------------------- /cmake-modules/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 | /opt/ros/$ENV{ROS_DISTRO}/include 13 | NO_DEFAULT_PATH 14 | ) 15 | 16 | # Macro to unify finding both the debug and release versions of the 17 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 18 | # macro. 19 | 20 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 21 | 22 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 23 | NAMES "g2o_${MYLIBRARYNAME}_d" 24 | PATHS 25 | ${G2O_ROOT}/lib/Debug 26 | ${G2O_ROOT}/lib 27 | $ENV{G2O_ROOT}/lib/Debug 28 | $ENV{G2O_ROOT}/lib 29 | NO_DEFAULT_PATH 30 | ) 31 | 32 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 33 | NAMES "g2o_${MYLIBRARYNAME}_d" 34 | PATHS 35 | ~/Library/Frameworks 36 | /Library/Frameworks 37 | /usr/local/lib 38 | /usr/local/lib64 39 | /usr/lib 40 | /usr/lib64 41 | /opt/local/lib 42 | /sw/local/lib 43 | /sw/lib 44 | ) 45 | 46 | FIND_LIBRARY(${MYLIBRARY} 47 | NAMES "g2o_${MYLIBRARYNAME}" 48 | PATHS 49 | ${G2O_ROOT}/lib/Release 50 | ${G2O_ROOT}/lib 51 | $ENV{G2O_ROOT}/lib/Release 52 | $ENV{G2O_ROOT}/lib 53 | NO_DEFAULT_PATH 54 | ) 55 | 56 | FIND_LIBRARY(${MYLIBRARY} 57 | NAMES "g2o_${MYLIBRARYNAME}" 58 | PATHS 59 | ~/Library/Frameworks 60 | /Library/Frameworks 61 | /usr/local/lib 62 | /usr/local/lib64 63 | /usr/lib 64 | /usr/lib64 65 | /opt/local/lib 66 | /sw/local/lib 67 | /sw/lib 68 | ) 69 | 70 | IF(NOT ${MYLIBRARY}_DEBUG) 71 | IF(MYLIBRARY) 72 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 73 | ENDIF(MYLIBRARY) 74 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 75 | 76 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 77 | 78 | # Find the core elements 79 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 80 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 81 | 82 | # Find the CLI library 83 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 84 | 85 | # Find the pluggable solvers 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 93 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 94 | 95 | # Find the predefined types 96 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 97 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 102 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 103 | 104 | # G2O solvers declared found if we found at least one solver 105 | SET(G2O_SOLVERS_FOUND "NO") 106 | 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) 107 | SET(G2O_SOLVERS_FOUND "YES") 108 | 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) 109 | 110 | # G2O itself declared found if we found the core libraries and at least one solver 111 | SET(G2O_FOUND "NO") 112 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 113 | SET(G2O_FOUND "YES") 114 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 115 | -------------------------------------------------------------------------------- /cmake-modules/FindOpenGV.cmake: -------------------------------------------------------------------------------- 1 | 2 | # This is FindOPENGV.cmake 3 | # CMake module to locate the OPENGV package 4 | # 5 | # The following cache variables may be set before calling this script: 6 | # 7 | # OPENGV_DIR (or OPENGV_ROOT): (Optional) The install prefix OR source tree of opengv (e.g. /usr/local or src/opengv) 8 | # OPENGV_BUILD_NAME: (Optional) If compiling against a source tree, the name of the build directory 9 | # within it (e.g build-debug). Without this defined, this script tries to 10 | # intelligently find the build directory based on the project's build directory name 11 | # or based on the build type (Debug/Release/etc). 12 | # 13 | # The following variables will be defined: 14 | # 15 | # OPENGV_FOUND : TRUE if the package has been successfully found 16 | # OPENGV_INCLUDE_DIR : paths to OPENGV's INCLUDE directories 17 | # OPENGV_LIBS : paths to OPENGV's libraries 18 | # 19 | # NOTES on compiling against an uninstalled OPENGV build tree: 20 | # - A OPENGV source tree will be automatically searched for in the directory 21 | # 'opengv' next to your project directory, after searching 22 | # CMAKE_INSTALL_PREFIX and $HOME, but before searching /usr/local and /usr. 23 | # - The build directory will be searched first with the same name as your 24 | # project's build directory, e.g. if you build from 'MyProject/build-optimized', 25 | # 'opengv/build-optimized' will be searched first. Next, a build directory for 26 | # your project's build type, e.g. if CMAKE_BUILD_TYPE in your project is 27 | # 'Release', then 'opengv/build-release' will be searched next. Finally, plain 28 | # 'opengv/build' will be searched. 29 | # - You can control the opengv build directory name directly by defining the CMake 30 | # cache variable 'OPENGV_BUILD_NAME', then only 'opengv/${OPENGV_BUILD_NAME} will 31 | # be searched. 32 | # - Use the standard CMAKE_PREFIX_PATH, or OPENGV_DIR, to find a specific opengv 33 | # directory. 34 | 35 | # Get path suffixes to help look for opengv 36 | if(OPENGV_BUILD_NAME) 37 | set(opengv_build_names "${OPENGV_BUILD_NAME}/opengv") 38 | else() 39 | # lowercase build type 40 | string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_suffix) 41 | # build suffix of this project 42 | get_filename_component(my_build_name "${CMAKE_BINARY_DIR}" NAME) 43 | 44 | set(opengv_build_names "${my_build_name}/opengv" "build-${build_type_suffix}/opengv" "build/opengv") 45 | endif() 46 | 47 | # Use OPENGV_ROOT or OPENGV_DIR equivalently 48 | if(OPENGV_ROOT AND NOT OPENGV_DIR) 49 | set(OPENGV_DIR "${OPENGV_ROOT}") 50 | endif() 51 | 52 | if(OPENGV_DIR) 53 | # Find include dirs 54 | find_path(OPENGV_INCLUDE_DIR opengv/types.hpp 55 | PATHS "${OPENGV_DIR}/include" "${OPENGV_DIR}" NO_DEFAULT_PATH 56 | DOC "OPENGV include directories") 57 | 58 | # Find libraries 59 | find_library(OPENGV_LIBS NAMES opengv 60 | HINTS "${OPENGV_DIR}/lib" "${OPENGV_DIR}" NO_DEFAULT_PATH 61 | PATH_SUFFIXES ${opengv_build_names} 62 | DOC "OPENGV libraries") 63 | else() 64 | # Find include dirs 65 | set(extra_include_paths ${CMAKE_INSTALL_PREFIX}/include "$ENV{HOME}/include" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/include /usr/include) 66 | find_path(OPENGV_INCLUDE_DIR opengv/types.hpp 67 | PATHS ${extra_include_paths} 68 | DOC "OPENGV include directories") 69 | if(NOT OPENGV_INCLUDE_DIR) 70 | message(STATUS "Searched for opengv headers in default paths plus ${extra_include_paths}") 71 | endif() 72 | 73 | # Find libraries 74 | find_library(OPENGV_LIBS NAMES opengv 75 | HINTS ${CMAKE_INSTALL_PREFIX}/lib "$ENV{HOME}/lib" "${PROJECT_SOURCE_DIR}/../opengv" /usr/local/lib /usr/lib 76 | PATH_SUFFIXES ${opengv_build_names} 77 | DOC "OPENGV libraries") 78 | endif() 79 | 80 | # handle the QUIETLY and REQUIRED arguments and set OPENGV_FOUND to TRUE 81 | # if all listed variables are TRUE 82 | include(FindPackageHandleStandardArgs) 83 | find_package_handle_standard_args(OPENGV DEFAULT_MSG 84 | OPENGV_LIBS OPENGV_INCLUDE_DIR) 85 | 86 | -------------------------------------------------------------------------------- /cmake-modules/dependencies.cmake: -------------------------------------------------------------------------------- 1 | # Find Boost Library 2 | find_package(Boost COMPONENTS system thread regex REQUIRED) 3 | #~ include_directories(${BOOST_INCLUDE_DIR}) 4 | 5 | # Find yaml-cpp Library 6 | find_package(PkgConfig) 7 | pkg_check_modules(YamlCpp yaml-cpp) 8 | 9 | # Find Eigen3 Library 10 | # Since it is header-only and it won't be linked, 11 | # we have to explicitly add the include directories. 12 | find_package(Eigen REQUIRED) 13 | include_directories(${EIGEN_INCLUDE_DIRS}) 14 | 15 | # Find OpenCV library 16 | #find_package(OpenCV REQUIRED xfeatures2d features2d core) 17 | 18 | ############################################################### 19 | # BUG FIX for OpenCV 3.3.1 in ROS Kinetic (see: https://github.com/ros-perception/vision_opencv/issues/193) 20 | ############################################################### 21 | 22 | find_package(OpenCV 3 REQUIRED) 23 | if (${OpenCV_VERSION} MATCHES "3.3.1") 24 | foreach(__cvcomponent ${OpenCV_LIB_COMPONENTS}) 25 | set (__original_cvcomponent ${__cvcomponent}) 26 | if(NOT __cvcomponent MATCHES "^opencv_") 27 | set(__cvcomponent opencv_${__cvcomponent}) 28 | endif() 29 | if (TARGET ${__cvcomponent}) 30 | set_target_properties(${__cvcomponent} PROPERTIES 31 | MAP_IMPORTED_CONFIG_DEBUG "" 32 | MAP_IMPORTED_CONFIG_RELEASE "" 33 | MAP_IMPORTED_CONFIG_RELWITHDEBINFO "" 34 | MAP_IMPORTED_CONFIG_MINSIZEREL "" 35 | ) 36 | endif() 37 | endforeach(__cvcomponent) 38 | endif() 39 | 40 | ############################################################### 41 | 42 | 43 | # Find Suitesparse library 44 | find_package(SuiteSparse REQUIRED) 45 | 46 | # Find G2O Library 47 | find_package(G2O REQUIRED) 48 | # select required components 49 | set(G2O_LIBRARIES ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SBA} ${G2O_TYPES_SLAM3D}) 50 | 51 | # Find libraries required by LoopClosing module 52 | if( USE_LOOPCLOSURE ) 53 | # DLoopDetector library (must be properly installed from github repos) 54 | find_package(DLib REQUIRED) 55 | find_package(DBoW2 REQUIRED) 56 | find_package(DLoopDetector REQUIRED) 57 | include_directories(${DLib_INCLUDE_DIRS} ${DBoW2_INCLUDE_DIRS} ${DLoopDetector_INCLUDE_DIRS}) 58 | set(DLD_LIBRARIES ${DLib_LIBRARIES} ${DBoW2_LIBRARIES}) # DLoopDetector its just a header 59 | 60 | # Find OpenGV 61 | find_package(OpenGV REQUIRED) 62 | include_directories(${OPENGV_INCLUDE_DIR}) 63 | 64 | # List of files to compile 65 | file(GLOB LC_SRCS src/sptam/loopclosing/*.cpp src/sptam/loopclosing/detectors/*.cpp) 66 | endif() 67 | -------------------------------------------------------------------------------- /configurationFiles/euroc_gftt_brief.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | 3 | Name: 'GFTT' 4 | 5 | nfeatures: 2000 6 | minDistance: 15.0 7 | qualityLevel: 0.001 8 | useHarrisDetector: false 9 | 10 | DescriptorExtractor: 11 | Name: 'BRIEF' 12 | bytes: 32 13 | 14 | DescriptorMatcher: 15 | # normType: use 16 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 17 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 18 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 19 | Name: 'BruteForce-Hamming' 20 | crossCheck: false 21 | 22 | MatchingCellSize: 15 23 | MatchingNeighborhood: 2 24 | MatchingDistance: 25 25 | EpipolarDistance: 1 26 | FrustumNearPlaneDist: 0.1 27 | FrustumFarPlaneDist: 50.0 28 | BundleAdjustmentActiveKeyframes: 10 29 | minimumTrackedPointsRatio: 0.9 30 | 31 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 32 | -------------------------------------------------------------------------------- /configurationFiles/euroc_orb_brief.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | 3 | Name: 'ORB' 4 | 5 | nFeatures: 200 6 | scaleFactor: 1.2 7 | nLevels: 1 8 | edgeThreshold: 31 9 | 10 | DescriptorExtractor: 11 | Name: 'BRIEF' 12 | bytes: 32 13 | 14 | DescriptorMatcher: 15 | # normType: use 16 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 17 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 18 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 19 | Name: 'BruteForce-Hamming' 20 | crossCheck: false 21 | 22 | MatchingCellSize: 15 23 | MatchingNeighborhood: 2 24 | MatchingDistance: 25 25 | EpipolarDistance: 1 26 | FrustumNearPlaneDist: 0.1 27 | FrustumFarPlaneDist: 50.0 28 | BundleAdjustmentActiveKeyframes: 10 29 | minimumTrackedPointsRatio: 0.9 30 | 31 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 32 | -------------------------------------------------------------------------------- /configurationFiles/kitti.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | 3 | Name: 'GFTT' 4 | 5 | nfeatures: 1000 6 | minDistance: 15.0 7 | qualityLevel: 0.01 8 | useHarrisDetector: false 9 | 10 | DescriptorExtractor: 11 | Name: 'BRIEF' 12 | bytes: 32 13 | 14 | # OpenCV3 15 | # use_orientation: false 16 | 17 | DescriptorMatcher: 18 | # normType: use 19 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 20 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 21 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 22 | Name: 'BruteForce-Hamming' 23 | crossCheck: false 24 | 25 | MatchingCellSize: 15 26 | MatchingNeighborhood: 2 27 | MatchingDistance: 25 28 | EpipolarDistance: 0 29 | FrustumNearPlaneDist: 0.1 30 | FrustumFarPlaneDist: 10000.0 31 | BundleAdjustmentActiveKeyframes: 10 32 | minimumTrackedPointsRatio: 0.9 33 | 34 | 35 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 36 | -------------------------------------------------------------------------------- /configurationFiles/kitti_cam_00_to_02_13_to_21.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | image_width: 1241 4 | image_height: 376 5 | 6 | camera_matrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 7.188560000000e+02, 0, 6.071928000000e+02, 0, 7.188560000000e+02, 1.852157000000e+02, 0, 0, 1 ] 11 | 12 | baseline: 5.3716571886e-01 13 | -------------------------------------------------------------------------------- /configurationFiles/kitti_cam_03.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | image_width: 1242 4 | image_height: 375 5 | 6 | camera_matrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 7.215377000000e+02, 0, 6.095593000000e+02, 0, 7.215377000000e+02, 1.728540000000e+02, 0, 0, 1 ] 11 | 12 | baseline: 5.3715058825e-01 13 | -------------------------------------------------------------------------------- /configurationFiles/kitti_cam_04_to_12.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | image_width: 1226 4 | image_height: 370 5 | 6 | camera_matrix: !!opencv-matrix 7 | rows: 3 8 | cols: 3 9 | dt: d 10 | data: [ 7.070912000000e+02, 0, 6.018873000000e+02, 0, 7.070912000000e+02, 1.831104000000e+02, 0, 0, 1 ] 11 | 12 | baseline: 5.3715065326e-01 13 | -------------------------------------------------------------------------------- /configurationFiles/kitti_gftt_brisk.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | 3 | Name: 'GFTT' 4 | 5 | nfeatures: 2000 6 | minDistance: 15.0 7 | qualityLevel: 0.01 8 | useHarrisDetector: false 9 | 10 | DescriptorExtractor: 11 | Name: 'BRISK' 12 | # OpenCV2 13 | # orientationNormalized: 'true' 14 | # scaleNormalized: 'true' 15 | # patternScale: '22.0' 16 | 17 | # OpenCV3 18 | # thresh: '30' 19 | # octaves: '3' 20 | # patternScale: '1.0' 21 | 22 | 23 | DescriptorMatcher: 24 | # normType: use 25 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 26 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 27 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 28 | Name: 'BruteForce-Hamming' 29 | crossCheck: false 30 | 31 | MatchingCellSize: 15 32 | MatchingNeighborhood: 2 33 | MatchingDistance: 100 34 | EpipolarDistance: 0 35 | FrustumNearPlaneDist: 0.1 36 | FrustumFarPlaneDist: 10000.0 37 | BundleAdjustmentActiveKeyframes: 10 38 | 39 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 40 | -------------------------------------------------------------------------------- /configurationFiles/level7.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | Name: 'GFTT' 3 | nfeatures: 1000 4 | minDistance: 15.0 5 | qualityLevel: 0.01 6 | useHarrisDetector: false 7 | 8 | DescriptorExtractor: 9 | Name: 'BRIEF' 10 | bytes: 32 11 | 12 | DescriptorMatcher: 13 | # normType: use 14 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 15 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 16 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 17 | Name: 'BruteForce-Hamming' 18 | crossCheck: false 19 | 20 | MatchingCellSize: 30 21 | MatchingNeighborhood: 2 22 | MatchingDistance: 25 23 | EpipolarDistance: 1 24 | FrustumNearPlaneDist: 0.1 25 | FrustumFarPlaneDist: 10000.0 26 | BundleAdjustmentActiveKeyframes: 10 27 | minimumTrackedPointsRatio: 0.9 28 | 29 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 30 | -------------------------------------------------------------------------------- /configurationFiles/mit.yaml: -------------------------------------------------------------------------------- 1 | FeatureDetector: 2 | 3 | Name: 'GFTT' 4 | 5 | nfeatures: 1000 6 | minDistance: 15.0 7 | qualityLevel: 0.01 8 | useHarrisDetector: false 9 | 10 | DescriptorExtractor: 11 | Name: 'BRIEF' 12 | bytes: 32 13 | 14 | DescriptorMatcher: 15 | # normType: use 16 | # - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 17 | # - NORM_HAMMING for ORB, BRISK, and BRIEF 18 | # - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 19 | Name: 'BruteForce-Hamming' 20 | crossCheck: false 21 | 22 | MatchingCellSize: 30 23 | MatchingNeighborhood: 2 24 | MatchingDistance: 25 25 | EpipolarDistance: 0 26 | FrustumNearPlaneDist: 0.1 27 | FrustumFarPlaneDist: 10000.0 28 | BundleAdjustmentActiveKeyframes: 10 29 | minimumTrackedPointsRatio: 0.9 30 | 31 | LoopDetectorVocabulary: 'SPTAM_PATH/bow_voc/DBoW2/brief_mit_malaga_vocabulary.yml.gz' 32 | -------------------------------------------------------------------------------- /launch/kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /launch/mit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sptam 4 | 0.0.0 5 | The sptam package 6 | 7 | taihu 8 | taihu 9 | 10 | GPLv3 11 | 12 | https://github.com/lrse/sptam 13 | 14 | Taihú Pire 15 | Thomas Fischer 16 | 17 | catkin 18 | 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | nav_msgs 24 | cv_bridge 25 | pcl_ros 26 | message_filters 27 | image_geometry 28 | tf2 29 | tf2_geometry_msgs 30 | tf2_ros 31 | cmake_modules 32 | nodelet 33 | libg2o 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /sptam_coordinate_systems.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lrse/sptam/4be0b75105cf1eacfc7c0856bf982e4e03c740e7/sptam_coordinate_systems.png -------------------------------------------------------------------------------- /sptam_coordinate_systems_prediction.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lrse/sptam/4be0b75105cf1eacfc7c0856bf982e4e03c740e7/sptam_coordinate_systems_prediction.png -------------------------------------------------------------------------------- /sptam_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | S-PTAM nodelet. 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project( sptam ) 3 | 4 | add_library(sptam_ros stereo_driver.cpp base_driver.cpp utils/tf_utils.cpp utils/opencv_parsers.cpp) 5 | 6 | add_library(sptam_nodelet sptam_nodelet.cpp) 7 | target_link_libraries(sptam_nodelet sptam_ros ${catkin_LIBRARIES} sptam) 8 | 9 | add_executable(sptam_node sptam_node.cpp) 10 | target_link_libraries(sptam_node sptam_ros ${catkin_LIBRARIES} sptam) 11 | -------------------------------------------------------------------------------- /src/ros/sptam_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | int main(int argc, char* argv[]) 38 | { 39 | ros::init(argc, argv, "sptam"); 40 | 41 | nodelet::Loader nodelet; 42 | nodelet::M_string remap( ros::names::getRemappings() ); 43 | nodelet::V_string nargv; 44 | std::string nodelet_name = ros::this_node::getName(); 45 | nodelet.load(nodelet_name, "sptam/sptam_nodelet", remap, nargv); 46 | 47 | ros::MultiThreadedSpinner spinner; 48 | spinner.spin(); 49 | 50 | return 0; 51 | } 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/ros/sptam_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include 35 | #include 36 | #include "stereo_driver.hpp" 37 | 38 | namespace sptam 39 | { 40 | class sptam_nodelet : public nodelet::Nodelet 41 | { 42 | public: 43 | 44 | void onInit() 45 | { 46 | NODELET_DEBUG("Initializing sptam nodelet..."); 47 | sptam_interface_.reset( new sptam::stereo_driver( getNodeHandle(), getPrivateNodeHandle() ) ); 48 | } 49 | 50 | private: 51 | 52 | std::unique_ptr sptam_interface_; 53 | }; 54 | } 55 | 56 | PLUGINLIB_EXPORT_CLASS(sptam::sptam_nodelet, nodelet::Nodelet) 57 | -------------------------------------------------------------------------------- /src/ros/utils/opencv_parsers.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of the ros-image-features package. 3 | * 4 | * Copyright (C) 2015 Taihú Pire and Thomas Fischer 5 | * For more information see 6 | * 7 | * S-PTAM 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 | * S-PTAM 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 S-PTAM. If not, see . 19 | * 20 | * Authors: Taihú Pire 21 | * Thomas Fischer 22 | * 23 | * Laboratory of Robotics and Embedded Systems 24 | * Department of Computer Science 25 | * Faculty of Exact and Natural Sciences 26 | * University of Buenos Aires 27 | */ 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | cv::Ptr loadFeatureDetector( ros::NodeHandle& nh, const std::string& detector_name, const std::string& base_name); 34 | 35 | cv::Ptr loadDescriptorExtractor( ros::NodeHandle& nh, const std::string& descriptor_name, const std::string& base_name); 36 | 37 | cv::Ptr loadDescriptorMatcher( ros::NodeHandle& nh, const std::string& matcher_name, const std::string& base_name); 38 | -------------------------------------------------------------------------------- /src/ros/utils/tf2eigen.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "../../sptam/utils/macros.hpp" 36 | #include 37 | #include 38 | #include 39 | 40 | inline Eigen::Vector3d tf2eigen(const tf2::Vector3& v) 41 | { 42 | return Eigen::Vector3d(v.x(), v.y(), v.z()); 43 | } 44 | 45 | inline Eigen::Matrix3d tf2eigen(const tf2::Matrix3x3& m) 46 | { 47 | Eigen::Matrix3d ret; 48 | 49 | forn(i, 3) forn(j, 3) ret(i, j) = m[i][j]; 50 | 51 | return ret; 52 | } 53 | -------------------------------------------------------------------------------- /src/ros/utils/tf_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | 38 | namespace tf2 39 | { 40 | 41 | void waitForTransform(const tf2_ros::Buffer& tf_buffer, 42 | const std::string& targetFrame, 43 | const std::string& sourceFrame, 44 | const ros::Time& time, 45 | const ros::Duration& timeout, 46 | tf2::Transform& sourceToTarget); 47 | 48 | void lookupTransform(const tf2_ros::Buffer& tf_buffer, 49 | const std::string& targetFrame, 50 | const std::string& sourceFrame, 51 | const ros::Time& time, 52 | tf2::Transform& sourceToTarget); 53 | 54 | bool lookupTransformSafe(const tf2_ros::Buffer& tf_buffer, 55 | const std::string& targetFrame, 56 | const std::string& sourceFrame, 57 | const ros::Time& time, 58 | tf2::Transform& sourceToTarget); 59 | 60 | } // tf2 61 | -------------------------------------------------------------------------------- /src/sptam/BundleDriver.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "g2o_driver.hpp" 36 | 37 | /** 38 | * The driver is an interface between the SPTAM framework and an external 39 | * bundle adjustment optimization routine. 40 | * optimizer setup and data format conversions are handled here. 41 | */ 42 | class BundleDriver 43 | { 44 | public: 45 | 46 | /** 47 | * Add the data to be used in the next adjustment call ( Adjust ) 48 | */ 49 | void SetData(ConstIterable&& adjustViews, ConstIterable&& fixedViews); 50 | 51 | /** 52 | * Perform bundle adjustment for the given parameters. 53 | * return true if BA finish, false if it was interrupted 54 | * 55 | * @return 56 | * return True if the process was explicitly interrupted by the user 57 | * (using Break()). False otherwise. 58 | */ 59 | bool Adjust(int maxIterations); 60 | 61 | /** 62 | * Load the parameters adjusted on the last adjustment call ( Adjust ) 63 | */ 64 | void SavePoints(); 65 | void SaveCameras(); 66 | 67 | /** 68 | * Handle Bad Measurements 69 | */ 70 | std::list< sptam::Map::SharedMeas > GetBadMeasurements(); 71 | 72 | private: 73 | 74 | G2ODriver minimizer_; 75 | 76 | std::vector point_vertices_; 77 | std::vector camera_vertices_; 78 | 79 | std::vector< sptam::Map::SharedMeas > measurements_; 80 | }; 81 | -------------------------------------------------------------------------------- /src/sptam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project( sptam ) 3 | 4 | FIND_PACKAGE(SuiteSparse) 5 | include_directories(${SUITESPARSE_INCLUDE_DIRS}) 6 | 7 | # List of files to compile 8 | file(GLOB SPTAM_SRCS *.cpp utils/*.cpp) 9 | 10 | # Compile the tracker visualization library 11 | if( SHOW_TRACKED_FRAMES ) 12 | set(SPTAM_SRCS ${SPTAM_SRCS} utils/draw/Draw.cpp) 13 | endif() 14 | 15 | # Compile the profiling library 16 | if( SHOW_PROFILING ) 17 | set(SPTAM_SRCS ${SPTAM_SRCS} utils/log/Logger.cpp) 18 | endif() 19 | 20 | # Build the sptam library 21 | add_library(sptam SHARED ${SPTAM_SRCS} ${LC_SRCS}) 22 | target_link_libraries(sptam ${OpenCV_LIBRARIES} ${G2O_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${CHOLMOD_LIBRARIES} cxsparse ${DLD_LIBRARIES} ${OPENGV_LIBS} ${YamlCpp_LIBRARIES} ${Boost_LIBRARIES} ${TBB_LIBRARIES}) 23 | 24 | set(SPTAM_SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) 25 | set(SPTAM_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR} PARENT_SCOPE) 26 | -------------------------------------------------------------------------------- /src/sptam/Camera.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #include "Camera.hpp" 34 | 35 | inline FrustumCulling computeFrustum(const CameraPose& pose, const CameraParameters& calibration) 36 | { 37 | return FrustumCulling( 38 | pose.GetPosition(), pose.GetOrientationQuaternion(), 39 | calibration.horizontalFov(), calibration.verticalFov(), 40 | calibration.frustumNearPlaneDistance(), calibration.frustumFarPlaneDistance() 41 | ); 42 | } 43 | 44 | inline Eigen::Matrix34d computeTransformation(const CameraPose& pose) 45 | { 46 | return inverseTransformation( computeTransformation(pose.GetOrientationMatrix(), pose.GetPosition()) ); 47 | } 48 | 49 | Camera::Camera(const CameraPose& pose, const CameraParameters& calibration) 50 | : pose_( pose ), calibration_( calibration ) 51 | , transformation_( computeTransformation( pose ) ) 52 | , projection_( calibration_.intrinsic() * transformation_ ) 53 | , frustum_( computeFrustum( pose, calibration ) ) 54 | {} 55 | 56 | void Camera::UpdatePose(const CameraPose& newPose) 57 | { 58 | pose_ = newPose; 59 | 60 | frustum_ = computeFrustum( pose_, calibration_ ); 61 | transformation_ = computeTransformation( newPose ); 62 | projection_ = calibration_.intrinsic() * transformation_; 63 | } 64 | 65 | std::ostream& operator << ( std::ostream& os, const Camera& camera) 66 | { 67 | return os << camera.GetPose(); 68 | } 69 | -------------------------------------------------------------------------------- /src/sptam/Camera.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "CameraPose.hpp" 37 | #include "CameraParameters.hpp" 38 | #include "FrustumCulling.hpp" 39 | 40 | class Camera 41 | { 42 | public: 43 | 44 | Camera(const CameraPose& pose, const CameraParameters& calibration); 45 | 46 | void UpdatePose(const CameraPose& newPose); 47 | 48 | inline const CameraPose& GetPose() const 49 | { return pose_; } 50 | 51 | inline const Eigen::Vector3d& GetPosition() const 52 | { return pose_.GetPosition(); } 53 | 54 | inline const Eigen::Quaterniond& GetOrientation() const 55 | { return pose_.GetOrientationQuaternion(); } 56 | 57 | inline const Eigen::Matrix3d& GetIntrinsics() const 58 | { return calibration_.intrinsic(); } 59 | 60 | inline const Eigen::Matrix34d& GetTransformation() const 61 | { return transformation_; } 62 | 63 | inline const Eigen::Matrix34d& GetProjection() const 64 | { return projection_; } 65 | 66 | inline const CameraParameters& GetCalibration() const 67 | { return calibration_; } 68 | 69 | inline bool CanView(const Eigen::Vector3d& pW) const 70 | { return frustum_.Contains( pW ); } 71 | 72 | private: 73 | 74 | CameraPose pose_; 75 | 76 | CameraParameters calibration_; 77 | 78 | // transform from world coordinates to camera coordinates. 79 | Eigen::Matrix34d transformation_; 80 | 81 | // transform from world coordinates to image coordinates. 82 | Eigen::Matrix34d projection_; 83 | 84 | FrustumCulling frustum_; 85 | }; 86 | 87 | std::ostream& operator << ( std::ostream& os, const Camera& camera); 88 | -------------------------------------------------------------------------------- /src/sptam/CameraPose.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "CameraPose.hpp" 35 | 36 | CameraPose::CameraPose() 37 | : position_( Eigen::Vector3d::Zero() ), orientation_( Eigen::Quaterniond::Identity() ) 38 | , orientationMatrix_( Eigen::Matrix3d::Identity() ), covariance_( Eigen::Matrix6d::Identity() ) 39 | {} 40 | 41 | CameraPose::CameraPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, const Eigen::Matrix6d& covariance) 42 | : position_( position ), orientation_( orientation ), covariance_( covariance ) 43 | { 44 | orientationMatrix_ = orientation_.toRotationMatrix(); 45 | } 46 | 47 | std::ostream& operator << ( std::ostream& os, const CameraPose& cameraPose) 48 | { 49 | const Eigen::Quaterniond& orientation = cameraPose.GetOrientationQuaternion(); 50 | return os << cameraPose.GetPosition() << " [" << orientation.x() << ", " << orientation.y() << ", " << orientation.z() << ", " << orientation.w() << "]"; 51 | } 52 | -------------------------------------------------------------------------------- /src/sptam/CameraPose.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | 37 | namespace Eigen 38 | { 39 | typedef Matrix Matrix6d; 40 | } 41 | 42 | class CameraPose 43 | { 44 | public: 45 | 46 | CameraPose(); 47 | 48 | /** 49 | * Build a camera pose from a position and an orientation represented 50 | * as a quaternion. 51 | */ 52 | CameraPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, const Eigen::Matrix6d& covariance); 53 | 54 | /** 55 | * Takes the representation of a point given in World coordinate 56 | * system (or whichever system the camera pose is relative to), and 57 | * returns the same point in the cameraPose's reference frame. 58 | */ 59 | /*inline cv::Point3d FromWorld( cv::Point3d x ) const 60 | { return rotationMatrix_ * x + cv::Point3d( translation_ ); }*/ 61 | 62 | /** 63 | * Takes the representation of a point given in the cameraPose's 64 | * reference frame, and returns the same point in World coordinate 65 | * system (or whichever system the camera pose is relative to). 66 | */ 67 | inline Eigen::Vector3d ToWorld( const Eigen::Vector3d& x ) const 68 | { return orientationMatrix_ * x + position_; } 69 | 70 | inline const Eigen::Vector3d& GetPosition() const 71 | { return position_; } 72 | 73 | inline const Eigen::Matrix3d& GetOrientationMatrix() const 74 | { return orientationMatrix_; } 75 | 76 | inline const Eigen::Quaterniond& GetOrientationQuaternion() const 77 | { return orientation_; } 78 | 79 | inline const Eigen::Matrix6d& covariance() const 80 | { return covariance_; } 81 | 82 | private: 83 | 84 | Eigen::Vector3d position_; 85 | 86 | Eigen::Quaterniond orientation_; 87 | 88 | Eigen::Matrix3d orientationMatrix_; 89 | 90 | Eigen::Matrix6d covariance_; 91 | }; 92 | 93 | std::ostream& operator << ( std::ostream& os, const CameraPose& cameraPose); 94 | -------------------------------------------------------------------------------- /src/sptam/CovisibilityWindow.cpp: -------------------------------------------------------------------------------- 1 | #include "CovisibilityWindow.hpp" 2 | #include "BundleDriver.hpp" 3 | #include "utils/log/ScopedProfiler.hpp" 4 | 5 | std::set selectAdjustableKeyframes(const std::list& new_keyframes, size_t n_adjust, std::function isSafe) 6 | { 7 | std::set< sptam::Map::SharedKeyFrame > keyframe_window; 8 | 9 | ////////////////////////// 10 | // Insert new keyframes // 11 | ////////////////////////// 12 | 13 | for ( auto keyframe : new_keyframes ) 14 | keyframe_window.insert( keyframe ); 15 | 16 | //////////////////////////////////////////// 17 | // Insert keyframes covisible to the last // 18 | //////////////////////////////////////////// 19 | 20 | const sptam::Map::SharedKeyFrame& last_keyframe = new_keyframes.back(); 21 | 22 | // get covisible keyframes for the query keyframe 23 | std::vector< std::pair > covisible_keyframes = last_keyframe->covisibilityKeyFramesVector(); 24 | 25 | // remove keyframes that are unsafe or already loaded 26 | const auto covisible_filtered_end = std::remove_if(covisible_keyframes.begin(), covisible_keyframes.end(), [&] (const auto& kv) { return kv.first->isFixed() or keyframe_window.count( kv.first ) or not isSafe( kv.first ); }); 27 | 28 | // calculate the size of potential covisible keyframes 29 | const size_t n_expected = n_adjust - keyframe_window.size(); 30 | const size_t n_avaible = std::distance(covisible_keyframes.begin(), covisible_filtered_end); 31 | const size_t n_covisibles = std::min(n_expected, n_avaible); 32 | const auto covisible_sorted_end = covisible_keyframes.begin() + n_covisibles; 33 | 34 | // extract the 'n_covisibles' keyframes with highest covisibility index 35 | std::partial_sort(covisible_keyframes.begin(), covisible_sorted_end, covisible_filtered_end, [](const auto& a, const auto& b) { return a.second > b.second; }); 36 | 37 | // insert selected keyframes into the window 38 | for(auto it=covisible_keyframes.begin(); it!=covisible_sorted_end; it++) 39 | keyframe_window.insert( it->first ); 40 | 41 | return keyframe_window; 42 | } 43 | 44 | sptam::Map::SharedKeyFrameSet selectSafeCovisibleKFeyframes(sptam::Map::SharedKeyFrameSet& query_keyframes, std::function isSafe) 45 | { 46 | sptam::Map::SharedKeyFrameSet covisible_keyframes; 47 | 48 | for ( const sptam::Map::SharedKeyFrame& keyFrame : query_keyframes ) 49 | for (auto& kv : keyFrame->covisibilityKeyFrames()) 50 | { 51 | const sptam::Map::SharedKeyFrame& covisible_keyframe = kv.first; 52 | 53 | if( !query_keyframes.count( covisible_keyframe ) ) 54 | if ( isSafe( covisible_keyframe ) ) 55 | covisible_keyframes.insert( covisible_keyframe ); 56 | } 57 | 58 | return covisible_keyframes; 59 | } 60 | 61 | void CovisibilityWindow::populateBA(const std::list& new_keyframes, BundleDriver& bundle_adjuster, sptam::Map::SharedKeyFrameSet& adjustable_keyframes, sptam::Map::SharedKeyFrameSet& fixed_keyframes, std::function isSafe) 62 | { 63 | #ifdef SHOW_PROFILING 64 | { 65 | sptam::ScopedProfiler timer(" ba local_select: "); 66 | #endif 67 | 68 | adjustable_keyframes = selectAdjustableKeyframes(new_keyframes, window_size_, isSafe); 69 | 70 | /* Any other keyframe inside the safe window that measure above points shared, will be used as fixed keyframe 71 | * Gaston: Loop Closure safe window its defined in the multi-threaded version through sincronization messages */ 72 | fixed_keyframes = selectSafeCovisibleKFeyframes(adjustable_keyframes, isSafe); 73 | 74 | #ifdef SHOW_PROFILING 75 | } 76 | #endif 77 | 78 | #ifdef SHOW_PROFILING 79 | { 80 | sptam::ScopedProfiler timer(" ba local_load: "); 81 | #endif 82 | 83 | bundle_adjuster.SetData( 84 | ConstSetIterable::from( adjustable_keyframes ), 85 | ConstSetIterable::from( fixed_keyframes ) 86 | ); 87 | 88 | #ifdef SHOW_PROFILING 89 | } 90 | #endif 91 | } 92 | -------------------------------------------------------------------------------- /src/sptam/CovisibilityWindow.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "ILocalWindow.hpp" 36 | 37 | // forward declaration 38 | class BundleDriver; 39 | 40 | class CovisibilityWindow : public ILocalWindow 41 | { 42 | public: 43 | 44 | CovisibilityWindow(size_t window_size) : window_size_( window_size ) {} 45 | 46 | void populateBA(const std::list& new_keyframes, BundleDriver& bundle_adjuster, sptam::Map::SharedKeyFrameSet& adjustable_keyframes, sptam::Map::SharedKeyFrameSet& fixed_keyframes, std::function isSafe) override; 47 | 48 | private: 49 | 50 | size_t window_size_; 51 | }; 52 | -------------------------------------------------------------------------------- /src/sptam/FeatureExtractorThread.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | 39 | #include "opencv2/core/version.hpp" 40 | #if CV_MAJOR_VERSION == 2 41 | #include 42 | #elif CV_MAJOR_VERSION == 3 43 | #include 44 | #endif 45 | 46 | class FeatureExtractorThread 47 | { 48 | public: 49 | 50 | /** 51 | * Launches a detection / description process for an image 52 | * in a separate thread. 53 | */ 54 | FeatureExtractorThread(const cv::Mat& image, 55 | cv::Ptr featureDetector, 56 | cv::Ptr descriptorExtractor 57 | , size_t nFeatures); 58 | 59 | /** 60 | * This call blocks until the detection / description process 61 | * has finished and the internal thread exits. 62 | */ 63 | inline void WaitUntilFinished() 64 | { featureExtractorThread_.join(); } 65 | 66 | /** 67 | * @brief 68 | * Get the computed keypoints. It is mandatory to call 69 | * WaitUntilFinished() before calling this function. 70 | */ 71 | inline const std::vector& GetKeyPoints() const 72 | { return keyPoints_; } 73 | 74 | /** 75 | * @brief 76 | * Get the computed descriptors. It is mandatory to call 77 | * WaitUntilFinished() before calling this function. 78 | */ 79 | inline const cv::Mat& GetDescriptors() const 80 | { return descriptors_; } 81 | 82 | private: 83 | 84 | cv::Mat image_; 85 | size_t nFeatures_; 86 | cv::Ptr featureDetector_, featureDetector2_; 87 | cv::Ptr descriptorExtractor_; 88 | 89 | std::thread featureExtractorThread_; 90 | 91 | cv::Mat descriptors_; 92 | std::vector keyPoints_; 93 | 94 | 95 | cv::Size grid_size; 96 | 97 | void Extract(void); 98 | void ExtractGrid(void); 99 | void quadtreeFilter(const std::list &keypoints_in, std::vector &keypoints_out); 100 | }; 101 | -------------------------------------------------------------------------------- /src/sptam/Frame.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "Frame.hpp" 35 | #include "utils/macros.hpp" 36 | #include "utils/projective_math.hpp" 37 | 38 | #if SHOW_PROFILING 39 | #include "utils/timer.h" 40 | #include "utils/log/Profiler.hpp" 41 | #endif 42 | 43 | Frame::Frame(const Camera& camera, const ImageFeatures& imageFeatures) 44 | : camera_( camera ), imageFeatures_( imageFeatures ) 45 | {} 46 | 47 | #define PROFILE_INTERNAL 1 48 | 49 | std::list > Frame::FindMatches(const std::aligned_vector& points, 50 | const std::vector& descriptors, 51 | const cv::DescriptorMatcher& descriptorMatcher, 52 | const double matchingDistanceThreshold, 53 | const size_t matchingNeighborhoodThreshold 54 | ) const 55 | { 56 | #if SHOW_PROFILING && PROFILE_INTERNAL 57 | sptam::Timer t_project; 58 | t_project.start(); 59 | #endif 60 | 61 | std::vector featurePredictions = project(camera_.GetProjection(), points); 62 | 63 | #if SHOW_PROFILING && PROFILE_INTERNAL 64 | t_project.stop(); 65 | WriteToLog(" xx FindMatchesFrame-project: ", t_project); 66 | #endif 67 | 68 | return imageFeatures_.FindMatches( 69 | featurePredictions, descriptors, descriptorMatcher, 70 | matchingDistanceThreshold, matchingNeighborhoodThreshold 71 | ); 72 | } 73 | -------------------------------------------------------------------------------- /src/sptam/Frame.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "Camera.hpp" 36 | #include "Measurement.hpp" 37 | #include "ImageFeatures.hpp" 38 | #include "utils/eigen_alignment.hpp" 39 | 40 | #include "opencv2/core/version.hpp" 41 | #if CV_MAJOR_VERSION == 2 42 | #include 43 | #elif CV_MAJOR_VERSION == 3 44 | #include 45 | #endif 46 | 47 | class Frame 48 | { 49 | public: 50 | 51 | Frame(const Camera& camera, const ImageFeatures& imageFeatures); 52 | 53 | inline const Eigen::Vector3d& GetPosition() const 54 | { return camera_.GetPosition(); } 55 | 56 | inline const Eigen::Quaterniond& GetOrientation() const 57 | { return camera_.GetOrientation(); } 58 | 59 | inline const Camera& GetCamera() const 60 | { return camera_; } 61 | 62 | inline const CameraPose& GetCameraPose() const 63 | { return camera_.GetPose(); } 64 | 65 | inline Eigen::Matrix34d GetProjection() const 66 | { return camera_.GetProjection(); } 67 | 68 | inline void UpdateCameraPose(const CameraPose& cameraPose) 69 | { camera_.UpdatePose( cameraPose ); } 70 | 71 | inline const ImageFeatures& GetFeatures() const{ 72 | return imageFeatures_; 73 | } 74 | 75 | /** 76 | * Match a set of 3D points with their respective descriptors 77 | * to the features in the current frame. 78 | */ 79 | std::list > FindMatches(const std::aligned_vector& points, 80 | const std::vector& descriptors, 81 | const cv::DescriptorMatcher& descriptorMatcher, 82 | const double matchingDistanceThreshold, 83 | const size_t matchingNeighborhoodThreshold 84 | ) const; 85 | 86 | inline void SetMatchedKeyPoint( size_t index ) const 87 | { imageFeatures_.SetMatchedKeyPoint( index ); } 88 | 89 | inline void GetUnmatchedKeyPoints(std::vector& keyPoints, cv::Mat& descriptors, std::vector& indexes) const 90 | { return imageFeatures_.GetUnmatchedKeyPoints(keyPoints, descriptors, indexes); } 91 | 92 | private: 93 | 94 | std::vector ComputeProjections(const std::vector& points); 95 | 96 | private: 97 | 98 | // the pose of the camera 99 | Camera camera_; 100 | 101 | // imageFeatures for matching during refinement 102 | // It's mutable because it marks matched features 103 | // to boost matching performance. 104 | // TODO Maybe there should be mutable some members of imageFeatures and no the whole object 105 | ImageFeatures imageFeatures_; 106 | }; 107 | -------------------------------------------------------------------------------- /src/sptam/FrustumCulling.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | 37 | /** 38 | * Class that represents a Viewing Frustum 39 | */ 40 | class FrustumCulling 41 | { 42 | public: 43 | 44 | FrustumCulling( 45 | const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, 46 | double horizontalFOV, double verticalFOV, double nearPlaneDist, double farPlaneDist 47 | ); 48 | 49 | /** 50 | * Test if a point lies inside the frustum or not 51 | */ 52 | bool Contains(const Eigen::Vector3d& point) const; 53 | 54 | /** 55 | * Debugging Function is used for daw the frustum with PCL 56 | */ 57 | void GetFarPlaneCorners(Eigen::Vector3d& bottomLeftCorner, Eigen::Vector3d& bottomRightCorner, Eigen::Vector3d& topLeftCorner, Eigen::Vector3d& topRightCorner); 58 | 59 | private: 60 | 61 | Eigen::Vector3d position_; 62 | Eigen::Matrix3d orientation_; 63 | 64 | Eigen::Vector4d nearPlane_; 65 | Eigen::Vector4d farPlane_; 66 | Eigen::Vector4d leftPlane_; 67 | Eigen::Vector4d rightPlane_; 68 | Eigen::Vector4d topPlane_; 69 | Eigen::Vector4d bottomPlane_; 70 | 71 | // far plane corners are saved to draw frustum when debugging. 72 | Eigen::Vector3d fp_bl; 73 | Eigen::Vector3d fp_br; 74 | Eigen::Vector3d fp_tl; 75 | Eigen::Vector3d fp_tr; 76 | 77 | void ComputeFrustum( 78 | double horizontalFOV, double verticalFOV, 79 | double nearPlaneDist, double farPlaneDist 80 | ); 81 | }; 82 | -------------------------------------------------------------------------------- /src/sptam/ILocalWindow.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include "Map.hpp" 37 | 38 | // forward declaration 39 | class BundleDriver; 40 | 41 | /** 42 | * @brief When performing Local Bundle Adjustment, a strategy for defining the 43 | * set of keyframes to be adjusted is necessary. Since this choice may vary for 44 | * different problems, it is desirable to be able to swiftly choose or switch 45 | * between different strategies. This class defines the common interface to 46 | * which every locality selection strategy must comply. 47 | */ 48 | class ILocalWindow 49 | { 50 | public: 51 | 52 | // TODO this should actually be the type that is expected by BA, shouldn't it? 53 | typedef std::set< sptam::Map::SharedKeyFrame > KeyFrameWindow; 54 | 55 | // TODO this is a hack because the current implementation of 56 | // CovisibilityWindow only looks for covisible keyframes to the LAST 57 | // inserted one. Ideally the strategy should search for the covisible 58 | // keyframes to all the new keyframes to be adjusted. Once this is the case, 59 | // we can eliminate the last parameter. We require that 60 | // last_keyframe \in new_keyframes to make the future transition easier. 61 | virtual void populateBA(const std::list& new_keyframes, BundleDriver& bundle_adjuster, sptam::Map::SharedKeyFrameSet& adjustable_keyframes, sptam::Map::SharedKeyFrameSet& fixed_keyframes, std::function isSafe) = 0; 62 | }; 63 | -------------------------------------------------------------------------------- /src/sptam/ImageFeatures.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include "utils/Hash2D.hpp" 38 | 39 | class ImageFeatures 40 | { 41 | public: 42 | 43 | ImageFeatures(const cv::Size& image_size, 44 | const std::vector keyPoints, 45 | const cv::Mat descriptors, 46 | const size_t MatchingCellSize); 47 | 48 | ImageFeatures(const ImageFeatures& imageFeatures); 49 | 50 | std::list > FindMatches( 51 | const std::vector& featurePredictions, 52 | const std::vector& descriptors, 53 | const cv::DescriptorMatcher& descriptorMatcher, 54 | const double matchingDistanceThreshold, 55 | const size_t matchingNeighborhoodThreshold 56 | ) const; 57 | 58 | inline cv::Mat GetDescriptors() const 59 | { return descriptors_; } 60 | 61 | inline cv::Mat GetDescriptor( size_t index ) const 62 | { return descriptors_.row( index ); } 63 | 64 | inline const std::vector& GetKeypoints() const 65 | { return keyPoints_; } 66 | 67 | inline const cv::KeyPoint& GetKeypoint( size_t index ) const 68 | { return keyPoints_[ index ]; } 69 | 70 | inline void SetMatchedKeyPoint( size_t index ) const 71 | { matchedKeyPoints_[ index ] = true; } 72 | 73 | void GetUnmatchedKeyPoints(std::vector& keyPoints, cv::Mat& descriptors, std::vector& indexes) const; 74 | 75 | private: 76 | 77 | mutable std::vector matchedKeyPoints_; 78 | 79 | cv::Mat descriptors_; 80 | 81 | std::vector keyPoints_; 82 | 83 | Hash2D hashed_indexes_; 84 | 85 | cv::Size image_size_; 86 | 87 | // we need this because the matching radius parameter is given in hash-cell units. 88 | size_t hash_cell_size_; 89 | 90 | // helper functions 91 | 92 | typedef std::pair iPair; 93 | 94 | iPair GetHash(const cv::Point2d& key, const size_t cellSize) const; 95 | 96 | int FindMatch( 97 | const cv::Point2d& prediction, 98 | const cv::Mat& descriptor, 99 | const cv::DescriptorMatcher& descriptorMatcher, 100 | const double matchingDistanceThreshold, 101 | const size_t matchingNeighborhoodThreshold 102 | ) const; 103 | }; 104 | -------------------------------------------------------------------------------- /src/sptam/MEAS.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | struct MEAS { cv::KeyPoint keypoint; cv::Mat descriptor; }; 39 | -------------------------------------------------------------------------------- /src/sptam/MapPoint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "MapPoint.hpp" 35 | #include "utils/projective_math.hpp" 36 | 37 | MapPoint::MapPoint(const Eigen::Vector3d& position, const Eigen::Vector3d& normal, const cv::Mat& descriptor, const Eigen::Matrix3d& covariance) 38 | : position_( position ) 39 | , covariance_( covariance ) 40 | , normal_( normal ) 41 | , outlierCount_( 0 ) 42 | , inlierCount_( 0 ) 43 | , projectionCount_( 0 ) 44 | , measurementCount_( 0 ) // is zero because, measurementCount_ is incremented always by Frame::AddMeasuement() 45 | , color_(255,255,255) 46 | { 47 | // TODO: do we have to clone? can't we just assign it? 48 | descriptor.copyTo(descriptor_); 49 | } 50 | 51 | /* When a points is added to the Map, has to be copyed, but the shared_mutex does cannot be 52 | * copied. Thats why we have to specify that the new points will have a separate mutex */ 53 | MapPoint::MapPoint(const MapPoint& mapPoint) 54 | : mpoint_mutex_() // The copy has a diferent mutex! 55 | , position_( mapPoint.position_ ) 56 | , covariance_( mapPoint.covariance_ ) 57 | , normal_( mapPoint.normal_ ) 58 | , outlierCount_( mapPoint.outlierCount_ ) 59 | , inlierCount_( mapPoint.inlierCount_ ) 60 | , projectionCount_( mapPoint.projectionCount_ ) 61 | , measurementCount_( mapPoint.measurementCount_ ) 62 | , color_( mapPoint.color_ ) 63 | { 64 | mapPoint.GetDescriptor().copyTo(descriptor_); 65 | } 66 | -------------------------------------------------------------------------------- /src/sptam/Match.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "Map.hpp" 36 | 37 | struct Match { sptam::Map::SharedPoint mapPoint; Measurement measurement; }; 38 | -------------------------------------------------------------------------------- /src/sptam/Measurement.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "Measurement.hpp" 35 | 36 | Measurement::Measurement(const Type& type, const Source& source, const cv::KeyPoint& keypoint, const cv::Mat& descriptor) 37 | : keypoints_({ keypoint }), descriptors_({descriptor}),type_( type ), source_(source) 38 | { 39 | // assert this is treated as a monocular measurement 40 | assert( type != Measurement::STEREO ); 41 | } 42 | 43 | Measurement::Measurement(const Source& source, const cv::KeyPoint& KeyPointLeft, const cv::Mat& descriptorLeft, const cv::KeyPoint& KeyPointRight, const cv::Mat& descriptorRight) 44 | : keypoints_({ KeyPointLeft, KeyPointRight }), descriptors_({descriptorLeft, descriptorRight}) , type_( Measurement::STEREO ), source_(source) 45 | { } 46 | 47 | Measurement::Measurement(const Type& type, const Source& source, const std::vector& keypoints, const std::vector& descriptors) 48 | : keypoints_(keypoints), type_(type), source_(source) 49 | { 50 | for(const auto& descriptor : descriptors) 51 | descriptors_.push_back(descriptor); // cv::Mat is just a pointer, data is not being copied 52 | } 53 | 54 | Measurement::Measurement(const Measurement& measurement) 55 | : keypoints_(measurement.GetKeypoints()), type_(measurement.GetType()), source_(measurement.GetSource()) 56 | { 57 | for(const auto& descriptor : measurement.GetDescriptors()) 58 | descriptors_.push_back(descriptor); // cv::Mat is just a pointer, data is not being copied 59 | } 60 | -------------------------------------------------------------------------------- /src/sptam/Measurement.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | 38 | class Measurement 39 | { 40 | public: 41 | 42 | // Possible measurement type 43 | typedef enum { STEREO, LEFT, RIGHT } Type; 44 | 45 | // Possible measurement sources 46 | typedef enum { SRC_TRIANGULATION, SRC_TRACKER, SRC_REFIND } Source; 47 | 48 | public: 49 | 50 | Measurement(const Type& type, const Source& source, const cv::KeyPoint& keypoint, const cv::Mat& descriptor); 51 | 52 | Measurement(const Source& source, const cv::KeyPoint& KeyPointLeft, const cv::Mat& descriptorLeft, const cv::KeyPoint& KeyPointRight, const cv::Mat& descriptorRight); 53 | 54 | Measurement(const Type& type, const Source& source, const std::vector& keyPoints, const std::vector& descriptors); 55 | 56 | Measurement(const Measurement& measurement); 57 | 58 | inline const std::vector& GetKeypoints() const 59 | { return keypoints_; } 60 | 61 | inline const cv::Mat& GetDescriptor() const 62 | { return descriptors_[0]; } 63 | 64 | inline const std::vector& GetDescriptors() const 65 | { return descriptors_; } 66 | 67 | inline const Type& GetType() const 68 | { return type_; } 69 | 70 | inline const Source& GetSource() const 71 | { return source_; } 72 | 73 | const cv::KeyPoint& GetMainKeypoint(void) const 74 | { return keypoints_[0]; } 75 | 76 | private: 77 | 78 | // Image feature position 79 | std::vector keypoints_; 80 | 81 | // Image feature descriptor 82 | // Data is not being copied, those cv::Mat are used as pointers. Don't pass temporary data! 83 | std::vector descriptors_; 84 | 85 | Type type_; 86 | 87 | Source source_; 88 | }; 89 | -------------------------------------------------------------------------------- /src/sptam/MotionModel.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "PosePredictor.hpp" 36 | #include 37 | 38 | /** 39 | * This motion model follows the one proposed in the master thesis of 40 | * Christof Hoppe: Large-Scale Robotic SLAM through Visual Mapping p.43 41 | * TODO (Maybe not anymore) 42 | */ 43 | class MotionModel : public PosePredictor 44 | { 45 | public: 46 | 47 | MotionModel(const ros::Time& time, const Eigen::Vector3d& initialPosition, const Eigen::Quaterniond& initialOrientation, const Eigen::Matrix6d& initialCovariance); 48 | 49 | // Get the current camera pose. 50 | void currentPose(Eigen::Vector3d& currentPosition, Eigen::Quaterniond& currentOrientation, Eigen::Matrix6d& covariance) const override; 51 | 52 | // Predict the next camera pose. 53 | void predictPose(const ros::Time& time, Eigen::Vector3d& predictedPosition, Eigen::Quaterniond& predictedOrientation, Eigen::Matrix6d& predictionCovariance) override; 54 | 55 | // Update the motion model given a new camera pose. 56 | void updatePose(const ros::Time& time, const Eigen::Vector3d& newPosition, const Eigen::Quaterniond& newOrientation, const Eigen::Matrix6d& covariance) override; 57 | 58 | // Reset the model given a new camera pose. Note: This method will be called when it happens an abrupt change in the pose (LoopClosing) 59 | void applyCorrection(const Eigen::Matrix4d& correction) override; 60 | 61 | private: 62 | 63 | bool initialized_; 64 | 65 | ros::Time last_update_; 66 | 67 | Eigen::Vector3d position_; 68 | 69 | Eigen::Quaterniond orientation_; 70 | 71 | Eigen::Matrix6d poseCovariance_; 72 | 73 | Eigen::Vector3d linearVelocity_; 74 | 75 | double angular_velocity_angle_; 76 | Eigen::Vector3d angular_velocity_axis_; 77 | 78 | public: 79 | 80 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 81 | }; 82 | -------------------------------------------------------------------------------- /src/sptam/ParallelFeatureExtractor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #if CV_MAJOR_VERSION == 2 37 | #include 38 | #include 39 | #elif CV_MAJOR_VERSION == 3 40 | #include 41 | #include 42 | #endif 43 | 44 | /** 45 | * Class to execute extract features in parallel 46 | */ 47 | class ParallelFeatureExtractor : public cv::ParallelLoopBody 48 | { 49 | 50 | public: 51 | ParallelFeatureExtractor( cv::Mat* images, 52 | cv::FeatureDetector* featureDetector, 53 | cv::DescriptorExtractor* descriptorExtractor, 54 | std::vector* keyPoints, 55 | cv::Mat* descriptors 56 | ) 57 | : images_( images ) 58 | , featureDetector_( featureDetector ) 59 | , descriptorExtractor_( descriptorExtractor ) 60 | , keyPoints_( keyPoints ) 61 | , descriptors_( descriptors ){} 62 | 63 | virtual void operator()( const cv::Range &r ) const { 64 | register cv::Mat* images = images_ + r.start; 65 | register std::vector* keyPoints = keyPoints_ + r.start; 66 | register cv::Mat* descriptors = descriptors_ + r.start; 67 | 68 | for (register int jf = r.start; jf != r.end; ++jf, ++images, ++keyPoints, ++descriptors) 69 | { 70 | // Compute Keypoints and descriptors 71 | 72 | featureDetector_->detect(*images, *keyPoints); 73 | 74 | descriptorExtractor_->compute(*images, *keyPoints, *descriptors); 75 | } 76 | } 77 | 78 | 79 | private: 80 | 81 | cv::Mat* images_; 82 | cv::FeatureDetector* featureDetector_; 83 | cv::DescriptorExtractor* descriptorExtractor_; 84 | 85 | cv::Mat* descriptors_; 86 | 87 | std::vector* keyPoints_; 88 | 89 | }; 90 | -------------------------------------------------------------------------------- /src/sptam/PosePredictor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | // ros::Time 36 | #ifdef BUILDING_ROS 37 | #include 38 | #else 39 | #include "../sptam/utils/time/time.h" 40 | #endif 41 | 42 | #include 43 | 44 | namespace Eigen 45 | { 46 | typedef Matrix Matrix6d; 47 | } 48 | 49 | /** 50 | * Abstract interface for pose prediction implementations. 51 | */ 52 | class PosePredictor 53 | { 54 | public: 55 | 56 | virtual ~PosePredictor() {} 57 | 58 | // Retrieve the last computed camera pose. 59 | // TODO use with care!!! This function should not even exists, it should be handled externally 60 | virtual void currentPose(Eigen::Vector3d& currentPosition, Eigen::Quaterniond& currentOrientation, Eigen::Matrix6d& covariance) const = 0; 61 | 62 | // Predict the next camera pose. 63 | // TODO can't be const because ekf internally modifies it's state when predicting. Should something there be mutable? 64 | virtual void predictPose(const ros::Time& time, Eigen::Vector3d& predictedPosition, Eigen::Quaterniond& predictedOrientation, Eigen::Matrix6d& predictionCovariance) /*const*/ = 0; 65 | 66 | // Update the motion model given a new camera pose. 67 | virtual void updatePose(const ros::Time& time, const Eigen::Vector3d& newPosition, const Eigen::Quaterniond& newOrientation, const Eigen::Matrix6d& covariance) = 0; 68 | 69 | // Apply transformation correction to the model. Note: This method will be called when it happens an abrupt change in the pose (LoopClosing) 70 | virtual void applyCorrection(const Eigen::Matrix4d& correction) = 0; 71 | }; 72 | -------------------------------------------------------------------------------- /src/sptam/RectifiedCameraParameters.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | 37 | namespace sptam{ 38 | 39 | struct RectifiedCameraParameters 40 | { 41 | const double baseline; 42 | const Eigen::Vector2d focal_length; 43 | const Eigen::Vector2d principal_point; 44 | 45 | RectifiedCameraParameters(const double baseline_param, 46 | const Eigen::Vector2d &focal_length_param, 47 | const Eigen::Vector2d &principal_point_param) 48 | : baseline(baseline_param) 49 | , focal_length(focal_length_param) 50 | , principal_point(principal_point_param) 51 | {} 52 | 53 | }; 54 | 55 | } // sptam 56 | -------------------------------------------------------------------------------- /src/sptam/StereoGraph.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | /* This file instantiates the CovisibilityGraph methods for compilation. 35 | * For more information on this, refer to the comment in utils/CovisibilityGraph.cpp */ 36 | 37 | #include "MapPoint.hpp" 38 | #include "StereoFrame.hpp" 39 | #include "Measurement.hpp" 40 | #include "utils/CovisibilityGraph.cpp" 41 | 42 | template class CovisibilityGraph; 43 | -------------------------------------------------------------------------------- /src/sptam/TrackerViewStereo.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "utils/draw/TrackerView.hpp" 37 | 38 | #include "opencv2/core/version.hpp" 39 | #if CV_MAJOR_VERSION == 2 40 | #include 41 | #elif CV_MAJOR_VERSION == 3 42 | #include 43 | #endif 44 | 45 | namespace sptam 46 | { 47 | 48 | class TrackerViewStereo : public TrackerView 49 | { 50 | public: 51 | 52 | TrackerViewStereo(const cv::Mat& image_left, const cv::Mat& image_right) 53 | : draw_output( DRAW_NONE ), image_left_( image_left ), image_right_( image_right ) 54 | {} 55 | 56 | void draw(const StereoFrame& frame, const sptam::Map::SharedMapPointList& filtered_points, 57 | const std::list& measurements, const Parameters& params, bool before_refine) override; 58 | 59 | cv::Vec3b featureColor(const Measurement& meas) const override; 60 | 61 | cv::Mat stereoFrameBeforeRefine, stereoFrameAfterRefine, 62 | leftFrameBeforeRefine, rightFrameBeforeRefine, 63 | leftFrameAfterRefine, rightFrameAfterRefine; 64 | 65 | /** 66 | * Specifies what kind of output is expected of the tracker 67 | */ 68 | enum DrawOutput 69 | { 70 | DRAW_NONE = 0, 71 | DRAW_BEFORE_REFINE_LEFT = (1 << 0), 72 | DRAW_BEFORE_REFINE_RIGHT = (1 << 1), 73 | DRAW_BEFORE_REFINE_STEREO = (DRAW_BEFORE_REFINE_LEFT | DRAW_BEFORE_REFINE_RIGHT), 74 | DRAW_AFTER_REFINE_LEFT = (1 << 2), 75 | DRAW_AFTER_REFINE_RIGHT = (1 << 3), 76 | DRAW_AFTER_REFINE_STEREO = (DRAW_AFTER_REFINE_LEFT | DRAW_AFTER_REFINE_RIGHT), 77 | }; 78 | 79 | void enableDrawOutput(DrawOutput output); 80 | 81 | private: 82 | 83 | DrawOutput draw_output; 84 | 85 | const cv::Mat image_left_, image_right_; 86 | }; 87 | 88 | } 89 | -------------------------------------------------------------------------------- /src/sptam/TrackingReport.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "CameraPose.hpp" 37 | #include "Map.hpp" 38 | 39 | /** 40 | * @brief Holds information regarding the result of a tracking operation 41 | */ 42 | class TrackingReport 43 | { 44 | public: 45 | 46 | TrackingReport() 47 | : T_corr(Eigen::Matrix4d::Identity()), state( State::OK ) 48 | {} 49 | 50 | CameraPose refinedCameraPose; 51 | 52 | /* Correction applied to the inputed estimatedCameraPose before refinement. 53 | * This corresponds to a loop correction applied to the ongoing trajectory */ 54 | Eigen::Matrix4d T_corr; 55 | 56 | sptam::Map::SharedMapPointList localMap; 57 | sptam::Map::SharedMapPointSet trackedMap; 58 | sptam::Map::SharedKeyFrameSet localKeyFrames; 59 | 60 | /** 61 | * Describes the possible outcomes of tracking 62 | */ 63 | enum class State { 64 | OK, /** tracking was succesful */ 65 | NOT_ENOUGH_POINTS /** there were not enough points for tracking */ 66 | }; 67 | State state; 68 | 69 | inline bool isOk() const 70 | { return state == TrackingReport::State::OK; } 71 | }; 72 | -------------------------------------------------------------------------------- /src/sptam/g2o_driver.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "MapPoint.hpp" 36 | #include "CameraPose.hpp" 37 | #include "Measurement.hpp" 38 | #include "RectifiedCameraParameters.hpp" 39 | 40 | #include 41 | 42 | class G2ODriver 43 | { 44 | public: 45 | 46 | typedef g2o::OptimizableGraph::Vertex Vertex; 47 | typedef g2o::OptimizableGraph::Edge Edge; 48 | 49 | G2ODriver(); 50 | 51 | /** 52 | * Perform bundle adjustment for the previously loaded objects. 53 | * 54 | * @return 55 | * return True if the process was explicitly interrupted by the user 56 | * (using Break()). False otherwise. 57 | */ 58 | bool Adjust(int maxIterations); 59 | 60 | // Driver SPTAM -> G2O 61 | 62 | /** 63 | * @brief TODO 64 | */ 65 | class VertexData : public g2o::OptimizableGraph::Data 66 | { 67 | public: 68 | 69 | //! read the data from a stream 70 | // I'm not sure what this is for, I'll leave it blank (tfischer) 71 | virtual bool read(std::istream& is) override 72 | { return true; } 73 | 74 | //! write the data to a stream 75 | // I'm not sure what this is for, I'll leave it blank (tfischer) 76 | virtual bool write(std::ostream& os) const override 77 | { return true; } 78 | 79 | virtual void saveData(g2o::OptimizableGraph::Vertex& vertex) = 0; 80 | }; 81 | 82 | Vertex* AddVertex( 83 | const CameraPose& cameraPose, const sptam::RectifiedCameraParameters& rectified_camera_parameters, 84 | const bool isFixed, VertexData* userData = nullptr 85 | ); 86 | 87 | Vertex* AddVertex(const MapPoint& mapPoint, const bool marginalize ,const bool isFixed, VertexData* userData = nullptr); 88 | 89 | void AddEdge(int edgeId, Vertex* point, Vertex* keyFrame, const Measurement& meas); 90 | 91 | // Driver G2O -> SPTAM 92 | 93 | static Eigen::Vector3d GetPoint( g2o::HyperGraph::Vertex& vertex ); 94 | 95 | static CameraPose GetPose( g2o::HyperGraph::Vertex& vertex, const Eigen::Matrix6d& covariance ); 96 | 97 | Eigen::Matrix6d GetPoseCovariance( const g2o::OptimizableGraph::Vertex& pose_vertex ); 98 | 99 | // ... 100 | 101 | inline const g2o::OptimizableGraph::EdgeContainer& activeEdges() 102 | { return optimizer_.activeEdges(); } 103 | 104 | private: 105 | 106 | // Id pool for the vertices 107 | int next_vertex_id_; 108 | 109 | // G2O optimizer 110 | 111 | g2o::SparseOptimizer optimizer_; 112 | 113 | // signal used by g2o to stop computing bundle adjustment 114 | 115 | bool optimizer_abort_request_; 116 | bool optimizer_abort_request_copy_; 117 | 118 | double gainTerminateThreshold_; 119 | 120 | double robust_cost_function_delta_; 121 | 122 | // helper functions 123 | 124 | Edge* BuildMonoEdge(const std::vector& projection); 125 | Edge* BuildMonoEdgeRight(const std::vector& projection); 126 | Edge* BuildStereoEdge(const std::vector& projection); 127 | }; 128 | -------------------------------------------------------------------------------- /src/sptam/loopclosing/LCDetector.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "../Map.hpp" 37 | 38 | /// Result of a detection 39 | struct DetectionMatch 40 | { 41 | /// Detection status. 42 | bool status; 43 | /// Query id 44 | size_t query; 45 | /// Matched id if loop detected, otherwise, best candidate 46 | size_t match; 47 | /// Matched score 48 | double score; 49 | 50 | /** Checks if the loop was detected */ 51 | inline bool detection() const 52 | { return status; } 53 | }; 54 | 55 | class LCDetector 56 | { 57 | public: 58 | virtual ~LCDetector(){} 59 | virtual DetectionMatch detectloop(const sptam::Map::SharedKeyFrame& stereo_frame) = 0; 60 | }; 61 | -------------------------------------------------------------------------------- /src/sptam/loopclosing/LoopClosing.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | // Eigen 40 | #include 41 | 42 | #include "LCDetector.hpp" 43 | #include "../Map.hpp" 44 | #include "../CameraParameters.hpp" 45 | #include "../utils/concurrent_queue.hpp" 46 | #include "../utils/eigen_alignment.hpp" 47 | 48 | // Forward declarations due to cross reference between sptam, loopclosing and mapmaker 49 | class SPTAM; 50 | class MapMakerThread; 51 | 52 | /** 53 | * 54 | */ 55 | class LoopClosing 56 | { 57 | public: 58 | /** 59 | * Collection of some tuning parameters 60 | * that are given to the LoopClosure 61 | */ 62 | struct Parameters 63 | { 64 | cv::Ptr descriptorMatcher; 65 | double matchingDistanceThreshold = 25.0; 66 | }; 67 | 68 | LoopClosing(SPTAM& tracker, MapMakerThread& mapper, sptam::Map& map, std::unique_ptr& detector, const Parameters& params); 69 | ~LoopClosing(); 70 | 71 | void addKeyFrame(sptam::Map::SharedKeyFrame& keyFrame); 72 | 73 | void addKeyFrames(const std::list< sptam::Map::SharedKeyFrame >& keyFrames); 74 | 75 | inline void stop() 76 | { stop_ = true; keyFrameQueue_.stop(); maintenanceThread_.join(); } 77 | 78 | private: 79 | 80 | SPTAM& tracker_; 81 | MapMakerThread& mapper_; 82 | sptam::Map& map_; 83 | std::unique_ptr loop_detector_; // LoopClosing is on charge of the detector! 84 | 85 | Parameters params_; 86 | 87 | std::aligned_vector> loop_frames_; 88 | 89 | concurrent_queue keyFrameQueue_; 90 | 91 | // Signal to stop the maintenance thread 92 | bool stop_ = false; 93 | 94 | // Thread running maintainance operations in paralell 95 | std::thread maintenanceThread_; 96 | 97 | void maintenance(); 98 | }; 99 | -------------------------------------------------------------------------------- /src/sptam/loopclosing/SmoothEstimatePropagator.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #ifndef __SMOOTH_ESTIMATE_PROPAGATOR_HPP__ 35 | #define __SMOOTH_ESTIMATE_PROPAGATOR_HPP__ 36 | 37 | #include 38 | 39 | // G2O 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | /* 46 | * SmoothEstimatePropagator propagates edge information relaxing it while moving away of fixed vertices. 47 | * Uses an exponential function for this, so a vertex near fixed vertices will be moved all the way accordingly 48 | * with edge restriction meanwhile a vertex far away will remain in the same place. 49 | * 50 | * Note that this propagator only works with VertexSE3 and EdgeSE3. 51 | */ 52 | 53 | class SmoothEstimatePropagator : g2o::EstimatePropagator 54 | { 55 | public: 56 | 57 | SmoothEstimatePropagator(g2o::SparseOptimizer* g, 58 | const double& maxDistance=std::numeric_limits::max(), 59 | const double& maxEdgeCost=std::numeric_limits::max()); 60 | 61 | void propagate(g2o::OptimizableGraph::Vertex* v); 62 | 63 | private: 64 | 65 | struct SmoothPropagateAction : g2o::EstimatePropagator::PropagateAction { 66 | public: 67 | SmoothPropagateAction(g2o::EstimatePropagator::AdjacencyMap* adj, const double& max_distance); 68 | 69 | void operator()(g2o::OptimizableGraph::Edge* e_, const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* to_) const; 70 | 71 | private: 72 | g2o::EstimatePropagator::AdjacencyMap* adjacency; 73 | double maxDistance; 74 | 75 | Eigen::Isometry3d exponencialInterpolation(const Eigen::Isometry3d& from, const Eigen::Isometry3d& to, double step) const; 76 | }; 77 | 78 | SmoothPropagateAction _smoothAction; 79 | g2o::EstimatePropagator::PropagateCost _treeCost; 80 | double _maxDistance, _maxEdgeCost; 81 | }; 82 | 83 | #endif //__SMOOTH_ESTIMATE_PROPAGATOR_HPP__ 84 | -------------------------------------------------------------------------------- /src/sptam/loopclosing/detectors/DLDLoopDetector.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "../LCDetector.hpp" 37 | 38 | // DLoopDetector and DBoW2 39 | #include "DBoW2.h" 40 | #include "FBRISK.h" 41 | #include "DLoopDetector.h" 42 | 43 | // F class of descriptor functions and definitions 44 | template 45 | class DLDLoopDetector : public LCDetector 46 | { 47 | typedef DBoW2::TemplatedVocabulary Vocabulary; 48 | typedef DLoopDetector::TemplatedLoopDetector LoopDetector; 49 | 50 | public: 51 | struct Parameters : LoopDetector::Parameters 52 | { 53 | Parameters(int height = 0, int width = 0, float frequency = 1, bool nss = true, 54 | float _alpha = 0.3, int _k = 0, 55 | DLoopDetector::GeometricalCheck geom = DLoopDetector::GEOM_NONE, int dilevels = 0) 56 | : LoopDetector::Parameters(height, width, frequency, nss, 57 | _alpha, _k, 58 | geom, dilevels) 59 | {} 60 | 61 | static void loadFromYML(const std::string& file_path); 62 | }; 63 | 64 | DLDLoopDetector(const std::string& voc_file_path, const Parameters& params); 65 | 66 | DetectionMatch detectloop(const sptam::Map::SharedKeyFrame& stereo_frame); 67 | 68 | private: 69 | // Descriptors Vocabulary 70 | Vocabulary voc; 71 | // Loop Detector 72 | LoopDetector detector; 73 | }; 74 | -------------------------------------------------------------------------------- /src/sptam/loopclosing/detectors/FBRISK.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * BRISK interface implementation 4 | * 5 | * Based on FORB.h implementation of 6 | * Dorian Galvez-Lopez in the DBoW2 project 7 | * 8 | * For more information on DBoW2 go to: 9 | * https://github.com/dorian3d/DBoW2.git 10 | * 11 | */ 12 | 13 | #ifndef __D_T_F_BRISK__ 14 | #define __D_T_F_BRISK__ 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "FClass.h" 21 | 22 | namespace DBoW2 { 23 | 24 | /// Functions to manipulate BRIEF descriptors 25 | class FBRISK: protected FClass 26 | { 27 | public: 28 | 29 | /// Descriptor type 30 | typedef cv::Mat TDescriptor; // CV_8U 31 | /// Pointer to a single descriptor 32 | typedef const TDescriptor *pDescriptor; 33 | /// Descriptor length (in bytes) 34 | static const int L = 64; 35 | 36 | /** 37 | * Calculates the mean value of a set of descriptors 38 | * @param descriptors 39 | * @param mean mean descriptor 40 | */ 41 | static void meanValue(const std::vector &descriptors, 42 | TDescriptor &mean); 43 | 44 | /** 45 | * Calculates the distance between two descriptors 46 | * @param a 47 | * @param b 48 | * @return distance 49 | */ 50 | static double distance(const TDescriptor &a, const TDescriptor &b); 51 | 52 | /** 53 | * Returns a string version of the descriptor 54 | * @param a descriptor 55 | * @return string version 56 | */ 57 | static std::string toString(const TDescriptor &a); 58 | 59 | /** 60 | * Returns a descriptor from a string 61 | * @param a descriptor 62 | * @param s string version 63 | */ 64 | static void fromString(TDescriptor &a, const std::string &s); 65 | 66 | /** 67 | * Returns a mat with the descriptors in float format 68 | * @param descriptors 69 | * @param mat (out) NxL 32F matrix 70 | */ 71 | static void toMat32F(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | /** 75 | * Returns a mat with the descriptors in float format 76 | * @param descriptors NxL CV_8U matrix 77 | * @param mat (out) NxL 32F matrix 78 | */ 79 | static void toMat32F(const cv::Mat &descriptors, cv::Mat &mat); 80 | 81 | /** 82 | * Returns a matrix with the descriptor in OpenCV format 83 | * @param descriptors vector of N row descriptors 84 | * @param mat (out) NxL CV_8U matrix 85 | */ 86 | static void toMat8U(const std::vector &descriptors, 87 | cv::Mat &mat); 88 | 89 | }; 90 | 91 | } // namespace DBoW2 92 | 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /src/sptam/match_to_points.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "match_to_points.hpp" 35 | #include "utils/eigen_alignment.hpp" 36 | 37 | #if SHOW_PROFILING 38 | #include "utils/timer.h" 39 | #include "utils/log/Profiler.hpp" 40 | #endif 41 | 42 | std::list matchToPoints( 43 | const StereoFrame& frame, ConstIterable&& mapPoints, 44 | const cv::Ptr descriptorMatcher, 45 | const size_t matchingNeighborhoodThreshold, 46 | const double matchingDistanceThreshold, 47 | const Measurement::Source source 48 | ) 49 | { 50 | #if SHOW_PROFILING 51 | sptam::Timer t_find; 52 | t_find.start(); 53 | #endif 54 | 55 | std::aligned_vector points; 56 | std::vector descriptors; 57 | 58 | points.reserve( mapPoints.size() ); 59 | descriptors.reserve( mapPoints.size() ); 60 | 61 | for ( const auto& mapPoint : mapPoints ) { 62 | points.push_back( mapPoint->GetPosition() ); 63 | descriptors.push_back( mapPoint->GetDescriptor() ); 64 | } 65 | 66 | std::list matchedIndexes; 67 | std::list measurements; 68 | 69 | frame.FindMatches(source, 70 | points, descriptors, 71 | *descriptorMatcher, 72 | matchingNeighborhoodThreshold, 73 | matchingDistanceThreshold, 74 | matchedIndexes, measurements 75 | ); 76 | 77 | std::list matches; 78 | 79 | size_t src_idx = 0; 80 | auto it_src = mapPoints.begin(); 81 | auto it_meas = measurements.begin(); 82 | for ( size_t idx : matchedIndexes ) 83 | { 84 | while( src_idx < idx ) { src_idx++; it_src++; } 85 | matches.push_back({*it_src, *it_meas}); 86 | it_meas++; 87 | } 88 | 89 | #if SHOW_PROFILING 90 | t_find.stop(); 91 | WriteToLog(" XX findMatches-find: ", t_find); 92 | #endif 93 | 94 | return matches; 95 | } 96 | -------------------------------------------------------------------------------- /src/sptam/match_to_points.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include 35 | #include "Match.hpp" 36 | #include "utils/Iterable.hpp" 37 | 38 | std::list matchToPoints( 39 | const StereoFrame& frame, ConstIterable&& mapPoints, 40 | const cv::Ptr descriptorMatcher, 41 | const size_t matchingNeighborhoodThreshold, 42 | const double matchingDistanceThreshold, 43 | const Measurement::Source source 44 | ); 45 | -------------------------------------------------------------------------------- /src/sptam/sptamParameters.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "opencv2/core/version.hpp" 36 | #if CV_MAJOR_VERSION == 2 37 | #include 38 | #elif CV_MAJOR_VERSION == 3 39 | #include 40 | #endif 41 | 42 | /** 43 | * Collection of some tuning parameters 44 | * that are given to sptam 45 | */ 46 | struct Parameters 47 | { 48 | cv::Ptr descriptorMatcher; 49 | 50 | // Matching variables 51 | int matchingCellSize = 30; 52 | int matchingNeighborhoodThreshold = 1; 53 | double matchingDistanceThreshold = 25.0; 54 | double epipolarDistanceThreshold = 0.0; 55 | 56 | // Local Bundle Adjustment parameters 57 | int nKeyFramesToAdjustByLocal = 10; 58 | int maxIterationsLocal = 20; 59 | 60 | // Keyframe policy variables 61 | double minimumTrackedPointsRatio = 0.9; 62 | 63 | // Algorithm-independent feature extraction parameters 64 | int nFeatures = 0; 65 | 66 | #ifdef USE_LOOPCLOSURE 67 | std::string loopDetectorVocabulary; 68 | #endif 69 | }; 70 | -------------------------------------------------------------------------------- /src/sptam/tracker_g2o.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "tracker_g2o.hpp" 35 | #include "g2o_driver.hpp" 36 | #include "types_sba_extension.hpp" 37 | 38 | #include "utils/macros.hpp" 39 | #include "utils/pose_covariance.hpp" 40 | #include "utils/projective_math.hpp" 41 | #include "utils/projection_derivatives.hpp" 42 | 43 | #include 44 | 45 | // ===================== // 46 | // Some magic numbers :) // 47 | // ===================== // 48 | 49 | // Minimum number of measurements required for tracking to operate correctly. 50 | #define MIN_MEAS_FOR_TRACKING 10 51 | 52 | // Maximum number of iterations we allow the BA to run. 53 | #define BA_MAX_ITERATIONS 10 54 | 55 | CameraPose tracker_g2o::RefineCameraPose( 56 | const CameraPose& estimatedCameraPose, 57 | const sptam::RectifiedCameraParameters& rectified_camera_parameters, 58 | const std::list& measurements 59 | ) 60 | { 61 | size_t measurements_num = measurements.size(); 62 | 63 | // We need at least 4 measurements to get a solution. 64 | // Make sure there are a bunch more just to be robust. 65 | if( measurements_num < MIN_MEAS_FOR_TRACKING ) 66 | throw not_enough_points(); 67 | 68 | G2ODriver minimizer; 69 | 70 | G2ODriver::Vertex* pose_vertex = minimizer.AddVertex(estimatedCameraPose, rectified_camera_parameters, false); 71 | 72 | // Add the points' 3D position 73 | 74 | for( const auto& match : measurements ) 75 | { 76 | G2ODriver::Vertex* point_vertex = minimizer.AddVertex(*match.mapPoint, false, true); 77 | 78 | // trivial edge id, since we won't need it for anything 79 | minimizer.AddEdge(0, point_vertex, pose_vertex, match.measurement); 80 | } 81 | 82 | if (!minimizer.Adjust( BA_MAX_ITERATIONS )) 83 | std::cout << "WARNING: reached BA_MAX_ITERATIONS in tracker during refine" << std::endl; 84 | 85 | const g2o::OptimizableGraph::Vertex& pose_graph_vertex = dynamic_cast( *pose_vertex ); 86 | 87 | return G2ODriver::GetPose( *pose_vertex, minimizer.GetPoseCovariance( pose_graph_vertex ) ); 88 | } 89 | -------------------------------------------------------------------------------- /src/sptam/tracker_g2o.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "Match.hpp" 36 | #include "CameraPose.hpp" 37 | 38 | namespace Eigen 39 | { 40 | typedef Matrix Matrix6d; 41 | } 42 | 43 | class tracker_g2o 44 | { 45 | public: 46 | 47 | /** 48 | * @brief This should be called for every frame of incoming stereo images. 49 | * This will use an estimate of the current camera pose and will try 50 | * to adjust it to the current map by minimizing reprojection errors. 51 | */ 52 | CameraPose RefineCameraPose( 53 | const CameraPose& estimatedCameraPose, 54 | const sptam::RectifiedCameraParameters& rectified_camera_parameters, 55 | const std::list& measurements 56 | ); 57 | 58 | class not_enough_points : public std::runtime_error 59 | { 60 | public: 61 | not_enough_points() 62 | : std::runtime_error("Not enough points for tracking.") {} 63 | }; 64 | }; 65 | -------------------------------------------------------------------------------- /src/sptam/types_sba_extension.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | 37 | namespace g2o { 38 | 39 | using namespace Eigen; 40 | 41 | // monocular projection for right camera 42 | // first two args are the measurement type, second two the connection classes 43 | class G2O_TYPES_SBA_API EdgeProjectP2MCRight : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexCam> 44 | { 45 | public: 46 | 47 | EdgeProjectP2MCRight(); 48 | virtual bool read(std::istream& is); 49 | virtual bool write(std::ostream& os) const; 50 | 51 | // return the error estimate as a 2-vector 52 | void computeError() 53 | { 54 | // from to 55 | const VertexSBAPointXYZ *point = static_cast(_vertices[0]); 56 | const VertexCam *cam = static_cast(_vertices[1]); 57 | 58 | 59 | // calculate the projection 60 | Vector4d pt; 61 | pt.head<3>() = point->estimate(); 62 | pt(3) = 1.0; 63 | const SBACam& nd = cam->estimate(); 64 | // these should be already ok 65 | /* nd.setTransform(); */ 66 | /* nd.setProjection(); */ 67 | /* nd.setDr(); */ 68 | 69 | Vector3d p = nd.w2n * pt; 70 | Vector3d pb(nd.baseline,0,0); 71 | 72 | // right camera px 73 | p = nd.Kcam*(p-pb); 74 | 75 | Vector2d perr; 76 | perr = p.head<2>()/p(2); 77 | // std::cout << std::endl << "CAM " << cam->estimate() << std::endl; 78 | // std::cout << "POINT " << pt.transpose() << std::endl; 79 | // std::cout << "PROJ " << p.transpose() << std::endl; 80 | // std::cout << "CPROJ " << perr.transpose() << std::endl; 81 | // std::cout << "MEAS " << _measurement.transpose() << std::endl; 82 | 83 | // error, which is backwards from the normal observed - calculated 84 | // _measurement is the measured projection 85 | _error = perr - _measurement; 86 | // std::cerr << _error.x() << " " << _error.y() << " " << chi2() << std::endl; 87 | } 88 | 89 | // jacobian 90 | virtual void linearizeOplus(); 91 | 92 | public: 93 | 94 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 95 | }; 96 | 97 | } // g2o 98 | -------------------------------------------------------------------------------- /src/sptam/utils/Hash2D.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "macros.hpp" 35 | 36 | #include 37 | #include 38 | 39 | template 40 | class Hash2D 41 | { 42 | public: 43 | 44 | Hash2D(size_t size_x, size_t size_y, size_t cell_size_x, size_t cell_size_y); 45 | 46 | inline size_t rows() const 47 | { return n_rows_; } 48 | 49 | inline size_t cols() const 50 | { return n_cols_; } 51 | 52 | /** 53 | * @brief insert element in bucket (x, y). 54 | */ 55 | void insert(size_t x, size_t y, const T& elem); 56 | 57 | std::list getNeighborhood(size_t x, size_t y, size_t radius) const; 58 | 59 | private: 60 | 61 | size_t n_rows_, n_cols_; 62 | 63 | size_t cell_size_x_, cell_size_y_; 64 | 65 | // The 2d hash matrix will be stored in 1D using row first order. 66 | // Each bucket has a list of the elements that fall into it. 67 | std::vector< std::list > hashed_elements_; 68 | 69 | /* 70 | * Get the bucket index for real coordinates (x, y). 71 | **/ 72 | inline size_t getHash(size_t x, size_t y) const 73 | { 74 | size_t cell_x, cell_y; 75 | getHash2D(x, y, cell_x, cell_y); 76 | return flatten(cell_x, cell_y); 77 | } 78 | 79 | inline void getHash2D(size_t x, size_t y, size_t& cell_x, size_t& cell_y) const 80 | { 81 | cell_x = floor((float) x / (float) cell_size_x_); 82 | cell_y = floor((float) y / (float) cell_size_y_); 83 | } 84 | 85 | inline size_t flatten(size_t cell_x, size_t cell_y) const 86 | { 87 | return cell_y * n_rows_ + cell_x; 88 | } 89 | }; 90 | 91 | template 92 | Hash2D::Hash2D(size_t size_x, size_t size_y, size_t cell_size_x, size_t cell_size_y) 93 | : n_rows_( ceil( size_y / (double) cell_size_y ) ) 94 | , n_cols_( ceil( size_x / (double) cell_size_x ) ) 95 | , cell_size_x_( cell_size_x ), cell_size_y_( cell_size_y ) 96 | , hashed_elements_( n_rows_ * n_cols_ ) 97 | {} 98 | 99 | template 100 | void Hash2D::insert(size_t x, size_t y, const T& elem) 101 | { 102 | size_t idx = getHash(x, y); 103 | assert( idx < hashed_elements_.size() ); 104 | hashed_elements_[ idx ].push_back( elem ); 105 | } 106 | 107 | template 108 | std::list Hash2D::getNeighborhood(size_t x, size_t y, size_t radius) const 109 | { 110 | size_t center_x, center_y; 111 | getHash2D(x, y, center_x, center_y); 112 | 113 | size_t fromY = radius < center_y ? center_y - radius : 0; 114 | size_t toY = std::min( center_y + radius + 1, rows() ); 115 | 116 | size_t fromX = radius < center_x ? center_x - radius : 0; 117 | size_t toX = std::min( center_x + radius + 1, cols() ); 118 | 119 | std::list ret; 120 | 121 | forsn (cell_y, fromY, toY) 122 | forsn (cell_x, fromX, toX) 123 | for( const T& elem : hashed_elements_[ flatten(cell_x, cell_y) ] ) 124 | ret.push_back( elem ); 125 | 126 | return ret; 127 | } 128 | -------------------------------------------------------------------------------- /src/sptam/utils/covariance_ellipsoid.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | // src: http://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/ 36 | 37 | #include 38 | 39 | enum prob_t {PROB_95=0, PROB_99}; 40 | 41 | // for 2D ellipses 42 | const double chi2_threshold_2D[2] = {5.991, 9.210}; 43 | 44 | // for 3D ellipses 45 | const double chi2_threshold_3D[2] = {7.815, 11.345}; 46 | 47 | struct Ellipse2D 48 | { 49 | double len1, len2; 50 | Eigen::Vector2d ax1, ax2; 51 | }; 52 | 53 | struct Ellipsoid3D 54 | { 55 | double len1, len2, len3; 56 | Eigen::Vector3d ax1, ax2, ax3; 57 | }; 58 | 59 | Ellipse2D computeCovarianceEllipse(const Eigen::Matrix2d& covariance, prob_t probability) 60 | { 61 | // A matrix that has real only entries is self adjoint (Hermitian) 62 | // iif it is symmetric. Since this holds for covariance matrices, 63 | // we can use a simpler solver. 64 | Eigen::SelfAdjointEigenSolver eigensolver; 65 | eigensolver.compute( covariance ); 66 | 67 | Ellipse2D ret; 68 | 69 | ret.ax1 = eigensolver.eigenvectors().col( 0 ); 70 | ret.ax2 = eigensolver.eigenvectors().col( 1 ); 71 | 72 | ret.len1 = sqrt( chi2_threshold_2D[ probability ] * eigensolver.eigenvalues()( 0 ) ); 73 | ret.len2 = sqrt( chi2_threshold_2D[ probability ] * eigensolver.eigenvalues()( 1 ) ); 74 | 75 | return ret; 76 | } 77 | 78 | Ellipsoid3D computeCovarianceEllipsoid(const Eigen::Matrix3d& covariance, prob_t probability) 79 | { 80 | // A matrix that has real only entries is self adjoint (Hermitian) 81 | // iif it is symmetric. Since this holds for covariance matrices, 82 | // we can use a simpler solver. 83 | Eigen::SelfAdjointEigenSolver eigensolver; 84 | eigensolver.compute( covariance ); 85 | 86 | Ellipsoid3D ret; 87 | 88 | ret.ax1 = eigensolver.eigenvectors().col( 0 ); 89 | ret.ax2 = eigensolver.eigenvectors().col( 1 ); 90 | ret.ax3 = eigensolver.eigenvectors().col( 2 ); 91 | 92 | ret.len1 = sqrt( chi2_threshold_3D[ probability ] * eigensolver.eigenvalues()( 0 ) ); 93 | ret.len2 = sqrt( chi2_threshold_3D[ probability ] * eigensolver.eigenvalues()( 1 ) ); 94 | ret.len3 = sqrt( chi2_threshold_3D[ probability ] * eigensolver.eigenvalues()( 2 ) ); 95 | 96 | return ret; 97 | } 98 | -------------------------------------------------------------------------------- /src/sptam/utils/cv2eigen.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "macros.hpp" 37 | 38 | #include 39 | #include 40 | #include "opencv2/core/version.hpp" 41 | #if CV_MAJOR_VERSION == 2 42 | #include 43 | #elif CV_MAJOR_VERSION == 3 44 | // #include 45 | #endif 46 | 47 | inline Eigen::Vector2d cv2eigen(const cv::Point2d& p) 48 | { 49 | return Eigen::Vector2d(p.x, p.y); 50 | } 51 | 52 | inline Eigen::Vector3d cv2eigen(const cv::Point3d& p) 53 | { 54 | return Eigen::Vector3d(p.x, p.y, p.z); 55 | } 56 | 57 | inline Eigen::Vector3d cv2eigen(const cv::Vec3d& v) 58 | { 59 | return Eigen::Vector3d(v[0], v[1], v[2]); 60 | } 61 | 62 | inline cv::Point2d eigen2cv(const Eigen::Vector2d& v) 63 | { 64 | return cv::Point2d(v[0], v[1]); 65 | } 66 | 67 | inline cv::Point3d eigen2cv(const Eigen::Vector3d& v) 68 | { 69 | return cv::Point3d(v[0], v[1], v[2]); 70 | } 71 | 72 | inline Eigen::Quaterniond cv2eigen(const cv::Vec4d& q) 73 | { 74 | return Eigen::Quaterniond(q[0], q[1], q[2], q[3]); 75 | } 76 | 77 | inline cv::Vec4d eigen2cv(const Eigen::Quaterniond& q) 78 | { 79 | return cv::Vec4d( q.w(), q.x(), q.y(), q.z() ); 80 | } 81 | 82 | template 83 | inline cv::Matx eigen2cv(const Eigen::Matrix& m) 84 | { 85 | cv::Matx ret; 86 | forn(i, ROWS) forn(j, COLS) 87 | ret(i, j) = m(i, j); 88 | return ret; 89 | } 90 | 91 | template 92 | inline Eigen::Matrix cv2eigen(const cv::Matx& m) 93 | { 94 | Eigen::Matrix ret; 95 | forn(i, ROWS) forn(j, COLS) 96 | ret(i, j) = m(i, j); 97 | return ret; 98 | } 99 | -------------------------------------------------------------------------------- /src/sptam/utils/draw/TrackerView.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "../../Match.hpp" 37 | #include "../../sptamParameters.hpp" 38 | #include "../../Map.hpp" 39 | 40 | namespace sptam 41 | { 42 | 43 | class TrackerView 44 | { 45 | public: 46 | 47 | virtual ~TrackerView() {} 48 | 49 | /** 50 | * Visualize tracking data. 51 | * @param before_refine if true, feature detection grid and unmatched keypoints are also drawn 52 | */ 53 | virtual void draw(const StereoFrame& frame, const sptam::Map::SharedMapPointList& filtered_points, 54 | const std::list& measurements, const Parameters& params, bool before_refine) = 0; 55 | 56 | virtual cv::Vec3b featureColor(const Measurement& meas) const = 0; 57 | }; 58 | 59 | } 60 | -------------------------------------------------------------------------------- /src/sptam/utils/eigen_alignment.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace std 42 | { 43 | template 44 | using aligned_vector = vector>; 45 | 46 | template 47 | using aligned_list = list>; 48 | 49 | template 50 | inline shared_ptr<_Tp> 51 | make_shared_aligned(_Args&&... __args) 52 | { 53 | typedef typename std::remove_const<_Tp>::type _Tp_nc; 54 | return std::allocate_shared<_Tp>(Eigen::aligned_allocator<_Tp_nc>(), std::forward<_Args>(__args)...); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/sptam/utils/fixed_queue.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include "Iterable.hpp" 37 | 38 | template 39 | class fixed_queue : public std::list 40 | { 41 | public: 42 | 43 | fixed_queue(size_t size) 44 | : max_size_( size ) 45 | {} 46 | 47 | void push(const T& value); 48 | 49 | private: 50 | 51 | size_t max_size_; 52 | }; 53 | 54 | template 55 | void fixed_queue::push(const T& value) 56 | { 57 | std::list::push_back( value ); 58 | 59 | if ( max_size_ < std::list::size() ) 60 | std::list::pop_front(); 61 | } 62 | -------------------------------------------------------------------------------- /src/sptam/utils/log/Logger.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "Logger.hpp" 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | Logger Logger::instance_; 41 | 42 | void Logger::Write(const std::string& message) 43 | { 44 | std::lock_guard lock( instance_.mutex_ ); 45 | instance_.logfile_ << message; 46 | } 47 | 48 | std::string getTimestampedFilename() 49 | { 50 | std::stringstream filename_stream; 51 | boost::posix_time::time_facet *facet = new boost::posix_time::time_facet("%Y-%m-%d_%H:%M:%S"); 52 | filename_stream.imbue(std::locale(std::cout.getloc(), facet)); 53 | filename_stream << boost::posix_time::second_clock::local_time() << ".log"; 54 | 55 | return filename_stream.str(); 56 | } 57 | 58 | Logger::Logger() 59 | { 60 | filename_ = getTimestampedFilename(); 61 | logfile_.open( filename_ ); 62 | logfile_ << std::setprecision(std::numeric_limits::digits10 + 1); 63 | } 64 | -------------------------------------------------------------------------------- /src/sptam/utils/log/Logger.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | class Logger 41 | { 42 | public: 43 | 44 | Logger(Logger const&) = delete; 45 | void operator = (Logger const&) = delete; 46 | 47 | static void Write( const std::string& ); 48 | 49 | static const std::string& FileName() 50 | { return instance_.filename_; } 51 | 52 | private: 53 | 54 | std::string filename_; 55 | 56 | std::ofstream logfile_; 57 | 58 | std::mutex mutex_; 59 | 60 | Logger(); 61 | 62 | // Singleton instance 63 | 64 | static Logger instance_; 65 | }; 66 | -------------------------------------------------------------------------------- /src/sptam/utils/log/ScopedProfiler.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "../timer.h" 37 | #include "Profiler.hpp" 38 | 39 | namespace sptam 40 | { 41 | 42 | class ScopedProfiler 43 | { 44 | public: 45 | 46 | ScopedProfiler(const std::string& tag) 47 | : tag_( tag ) 48 | { timer_.start(); } 49 | 50 | ~ScopedProfiler() 51 | { 52 | timer_.stop(); 53 | WriteToLog(tag_, timer_); 54 | } 55 | 56 | private: 57 | 58 | const std::string tag_; 59 | 60 | sptam::Timer timer_; 61 | }; 62 | 63 | } 64 | -------------------------------------------------------------------------------- /src/sptam/utils/macros.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | /** useful macros */ 37 | #define forn(i,n) for(size_t i=0;i<(n);i++) 38 | #define fornr(i,n) for(size_t i=(n)-1;0<=i;i--) 39 | #define forsn(i,s,n) for(size_t i=(s);i<(n);i++) 40 | #define forsnr(i,s,n) for(size_t i=(n)-1;(s)<=i;i--) 41 | #define forall(it,X) for(decltype((X).begin()) it=(X).begin();it!=(X).end();it++) 42 | #define forallr(it,X) for(decltype((X).rbegin()) it=(X).rbegin();it!=(X).rend();it++) 43 | -------------------------------------------------------------------------------- /src/sptam/utils/pose_covariance.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | 37 | namespace Eigen 38 | { 39 | typedef Matrix Matrix6d; 40 | } 41 | 42 | /** 43 | * @brief Compute how much a single measurement contributes to the Camera Pose covariance. 44 | */ 45 | Eigen::Matrix6d computeMeasurementCovariance(double mu_x, double mu_y, double mu_z, double mu_roll, double mu_pitch, double mu_yaw, const Eigen::Vector3d& xw, const Eigen::Vector3d& xc, const Eigen::Vector2d& z, const Eigen::Matrix3d& K, const Eigen::Matrix2d& cov_z); 46 | -------------------------------------------------------------------------------- /src/sptam/utils/projection_derivatives.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | //////////////////////////////////////////////////////////////////////////////// 35 | // Jacobian computation functions. functions with a '2' suffix are the versions 36 | // using my own calculations for what should be the projection derivatives. 37 | // The other functions are the versions given and used by by g2o for BA. 38 | 39 | #include 40 | 41 | namespace Eigen 42 | { 43 | typedef Matrix Matrix9d; 44 | } 45 | 46 | Eigen::Matrix jacobianXj(const Eigen::Matrix& camera_transform, const Eigen::Matrix3d& Kcam, const Eigen::Vector3d& point_world); 47 | 48 | Eigen::Matrix jacobianXi(const Eigen::Matrix& camera_transform, const Eigen::Matrix3d& Kcam, const Eigen::Vector3d& point_world); 49 | 50 | //////////////////////////////////////////////////////////////////////////////// 51 | -------------------------------------------------------------------------------- /src/sptam/utils/set_union.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | template 4 | std::set setUnion(const std::set& s1, const std::set& s2) 5 | { 6 | std::set ret; 7 | 8 | auto it1 = s1.begin(); 9 | auto it2 = s2.begin(); 10 | 11 | while (it1 != s1.end() or it2 != s2.end()) 12 | { 13 | if ( it2==s2.end() or (it1!=s1.end() and *it1 < *it2) ) { 14 | ret.insert(ret.end(), *it1); 15 | it1++; 16 | } else { 17 | ret.insert(ret.end(), *it2); 18 | it2++; 19 | } 20 | } 21 | 22 | return ret; 23 | } 24 | -------------------------------------------------------------------------------- /src/sptam/utils/timer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #include 34 | #include 35 | #include 36 | #include "timer.h" 37 | 38 | sptam::Timer::Timer(void) : 39 | elapsed_seconds(0), started(false) 40 | { 41 | 42 | } 43 | 44 | void sptam::Timer::start(void) 45 | { 46 | assert(!started); 47 | t = clock_t::now(); 48 | started = true; 49 | } 50 | 51 | void sptam::Timer::stop(void) 52 | { 53 | assert(started); 54 | elapsed_seconds += std::chrono::duration(clock_t::now() - t).count() * 1e-3; 55 | started = false; 56 | } 57 | 58 | double sptam::Timer::elapsed(void) const 59 | { 60 | return elapsed_seconds; 61 | } 62 | 63 | double sptam::Timer::now() 64 | { 65 | return std::chrono::duration_cast(clock_t::now().time_since_epoch()).count() * 1e-6; 66 | } 67 | 68 | 69 | std::ostream& operator<< (std::ostream& stream, const sptam::Timer& t) 70 | { 71 | stream << std::setprecision(16) << std::fixed << t.elapsed(); 72 | return stream; 73 | } 74 | -------------------------------------------------------------------------------- /src/sptam/utils/timer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | namespace sptam 39 | { 40 | class Timer 41 | { 42 | public: 43 | Timer(void); 44 | 45 | void start(void); /* measures initial time */ 46 | void stop(void); /* measures elapsed time */ 47 | double elapsed(void) const; /* returns elapsed time in seconds */ 48 | 49 | static double now(void); /* returns seconds since epoch */ 50 | 51 | private: 52 | typedef std::chrono::high_resolution_clock clock_t; 53 | std::chrono::time_point t; 54 | double elapsed_seconds; 55 | bool started; 56 | }; 57 | } 58 | 59 | std::ostream& operator<< (std::ostream& stream, const sptam::Timer& t); 60 | 61 | -------------------------------------------------------------------------------- /src/standAlone/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /src/standAlone/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project( sptam ) 3 | 4 | # X11 library is required for multithreaded visualization in OpenCV 5 | if( SHOW_TRACKED_FRAMES ) 6 | find_package(X11 REQUIRED) 7 | include_directories(${X11_INCLUDE_DIRS}) 8 | endif() 9 | 10 | # Link to standard threading library 11 | if( NOT SINGLE_THREAD ) 12 | set(THREAD_LIBS pthread) 13 | endif() 14 | 15 | # Compile frame generator library 16 | file(GLOB frameGeneratorFiles FrameGenerator/*.cpp) 17 | add_library(frameGenerator ${frameGeneratorFiles}) 18 | target_link_libraries(frameGenerator ${OpenCV_LIBS}) 19 | 20 | file(GLOB STANDALONE_SRCS StereoFeatureExtractor.cpp KITTIGroundTruth.cpp SptamWrapper.cpp utils/ProgramOptions.cpp Timestamps.cpp) 21 | 22 | add_executable(sptam-stereo sptam-stereo.cpp ${STANDALONE_SRCS} ${SLAM_SRCS}) 23 | target_link_libraries(sptam-stereo sptam frameGenerator ${X11_LIBRARIES} boost_program_options ${THREAD_LIBS}) 24 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/FileSequenceFrameGenerator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "FileSequenceFrameGenerator.h" 35 | 36 | #include 37 | 38 | #include "opencv2/core/version.hpp" 39 | #if CV_MAJOR_VERSION == 2 40 | #include 41 | #elif CV_MAJOR_VERSION == 3 42 | #include 43 | #include 44 | #endif 45 | 46 | FileSequenceFrameGenerator::FileSequenceFrameGenerator(const std::string& path, size_t imageBeginIndex, size_t imageEndIndex) 47 | : imageBeginIndex_( imageBeginIndex ), imageEndIndex_( imageEndIndex ), imageActualIndex_( imageBeginIndex ) 48 | { 49 | cv::glob(path, filenames_); 50 | } 51 | 52 | bool FileSequenceFrameGenerator::getNextFrame(cv::Mat& frame) 53 | { 54 | // if there is not more images, return false 55 | if (imageActualIndex_ > imageEndIndex_ or imageActualIndex_ == filenames_.size() ) { 56 | return false; 57 | } 58 | 59 | // get image path 60 | const std::string& filename = filenames_[imageActualIndex_]; 61 | 62 | // read image 63 | frame = cv::imread( filename ); 64 | 65 | if (not frame.data) 66 | throw std::invalid_argument("Unable to read image: " + filename); 67 | 68 | // move image index to the next one 69 | imageActualIndex_++; 70 | 71 | return true; 72 | } 73 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/FileSequenceFrameGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "IFrameGenerator.h" 36 | 37 | class FileSequenceFrameGenerator : public IFrameGenerator 38 | { 39 | public: 40 | 41 | FileSequenceFrameGenerator(const std::string& path, size_t imageBeginIndex, size_t imageEndIndex); 42 | 43 | bool getNextFrame(cv::Mat& frame) override; 44 | 45 | private: 46 | 47 | std::vector filenames_; 48 | 49 | size_t imageBeginIndex_; 50 | size_t imageEndIndex_; 51 | size_t imageActualIndex_; 52 | }; 53 | 54 | #pragma once 55 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/FrameGeneratorFactory.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "IFrameGenerator.h" 36 | #include "VideoFileFrameGenerator.h" 37 | #include "ListOfFilesFrameGenerator.h" 38 | #include "FileSequenceFrameGenerator.h" 39 | 40 | std::unique_ptr createFrameGenerator( 41 | const std::string& imagesSource, 42 | const std::string& imagesSourceType, 43 | const size_t imageBeginIndex = 0, 44 | const size_t imageEndIndex = std::numeric_limits::max() 45 | ){ 46 | if ( imagesSourceType.compare("vid") == 0 ) { 47 | return std::make_unique( imagesSource ); 48 | } 49 | else if ( imagesSourceType.compare("dir") == 0 ) { 50 | return std::make_unique( imagesSource, imageBeginIndex, imageEndIndex); 51 | } 52 | else if ( imagesSourceType.compare("list" ) == 0) { 53 | return std::make_unique( imagesSource ); 54 | } 55 | 56 | throw std::invalid_argument("Images source type is not correct"); 57 | } 58 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/IFrameGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | #include "opencv2/core/version.hpp" 39 | #if CV_MAJOR_VERSION == 2 40 | #include 41 | #elif CV_MAJOR_VERSION == 3 42 | #include 43 | #endif 44 | 45 | 46 | class IFrameGenerator 47 | { 48 | public: 49 | 50 | virtual ~IFrameGenerator(){}; 51 | 52 | virtual bool getNextFrame(cv::Mat& frame) = 0; 53 | }; 54 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/ListOfFilesFrameGenerator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "ListOfFilesFrameGenerator.h" 35 | 36 | #include 37 | #include 38 | 39 | #include "opencv2/core/version.hpp" 40 | #if CV_MAJOR_VERSION == 2 41 | #include 42 | #elif CV_MAJOR_VERSION == 3 43 | #include 44 | #include 45 | #endif 46 | 47 | ListOfFilesFrameGenerator::ListOfFilesFrameGenerator(const std::string& filename) 48 | { 49 | std::ifstream file( filename ); 50 | 51 | std::string line; 52 | while ( std::getline(file, line) ) 53 | filenames_.push_back( line ); 54 | 55 | next_ = filenames_.begin(); 56 | } 57 | 58 | bool ListOfFilesFrameGenerator::getNextFrame(cv::Mat& frame) 59 | { 60 | if ( next_ == filenames_.end() ) 61 | return false; 62 | 63 | frame = cv::imread( *next_ ); 64 | 65 | if (not frame.data) 66 | throw std::invalid_argument("Unable to read image: " + *next_); 67 | 68 | ++next_; 69 | 70 | return true; 71 | } 72 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/ListOfFilesFrameGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "IFrameGenerator.h" 37 | #include 38 | 39 | class ListOfFilesFrameGenerator : public IFrameGenerator 40 | { 41 | public: 42 | 43 | ListOfFilesFrameGenerator(const std::string& filename); 44 | ~ListOfFilesFrameGenerator() {}; 45 | 46 | bool getNextFrame(cv::Mat& frame) override; 47 | 48 | private: 49 | 50 | std::list filenames_; 51 | std::list::const_iterator next_; 52 | }; 53 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/VideoFileFrameGenerator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "VideoFileFrameGenerator.h" 35 | 36 | VideoFileFrameGenerator::VideoFileFrameGenerator(const std::string& fileName) 37 | : videoCapture_() 38 | { 39 | videoCapture_.open( fileName ); 40 | 41 | if (not videoCapture_.isOpened()) 42 | throw std::invalid_argument("Unable to read vide: " + fileName); 43 | } 44 | 45 | VideoFileFrameGenerator::~VideoFileFrameGenerator() 46 | { 47 | videoCapture_.release(); 48 | } 49 | 50 | bool VideoFileFrameGenerator::getNextFrame(cv::Mat& frame) 51 | { 52 | return videoCapture_.read( frame ); 53 | } 54 | -------------------------------------------------------------------------------- /src/standAlone/FrameGenerator/VideoFileFrameGenerator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include "IFrameGenerator.h" 37 | 38 | #include 39 | 40 | #include "opencv2/core/version.hpp" 41 | #if CV_MAJOR_VERSION == 2 42 | #include 43 | #elif CV_MAJOR_VERSION == 3 44 | #include 45 | #include 46 | #endif 47 | 48 | class VideoFileFrameGenerator : public IFrameGenerator 49 | { 50 | public: 51 | 52 | VideoFileFrameGenerator(const std::string& fileName); 53 | ~VideoFileFrameGenerator(); 54 | 55 | bool getNextFrame(cv::Mat& frame) override; 56 | 57 | private: 58 | 59 | cv::VideoCapture videoCapture_; 60 | }; 61 | -------------------------------------------------------------------------------- /src/standAlone/KITTIGroundTruth.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "../sptam/PosePredictor.hpp" 40 | #include "../sptam/utils/eigen_alignment.hpp" 41 | 42 | class KITTIGroundTruth : public PosePredictor 43 | { 44 | public: 45 | 46 | KITTIGroundTruth(const std::string& filename); 47 | 48 | // Get the curent camera pose. 49 | void currentPose(Eigen::Vector3d& currentPosition, Eigen::Quaterniond& currentOrientation, Eigen::Matrix6d& covariance) const override; 50 | 51 | // Predict the next camera pose. 52 | void predictPose(const ros::Time& time, Eigen::Vector3d& predictedPosition, Eigen::Quaterniond& predictedOrientation, Eigen::Matrix6d& predictionCovariance) override; 53 | 54 | // Update the motion model given a new camera pose. 55 | void updatePose(const ros::Time& time, const Eigen::Vector3d& newPosition, const Eigen::Quaterniond& newOrientation, const Eigen::Matrix6d& covariance) override; 56 | 57 | // Reset the model given a new camera pose. Note: This method will be called when it happens an abrupt change in the pose (LoopClosing) 58 | void applyCorrection(const Eigen::Matrix4d& correction) override; 59 | 60 | std::aligned_vector positions_; 61 | std::aligned_vector orientations_; 62 | 63 | private: 64 | 65 | mutable size_t currentFrameIndex_; 66 | 67 | Eigen::Vector3d currentPosition_; 68 | Eigen::Quaterniond currentOrientation_; 69 | 70 | public: 71 | 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | }; 74 | -------------------------------------------------------------------------------- /src/standAlone/SptamWrapper.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "../sptam/sptam.hpp" 36 | #include "../sptam/PosePredictor.hpp" 37 | #include "StereoImageFeatures.hpp" 38 | 39 | class SptamWrapper 40 | { 41 | public: 42 | 43 | SptamWrapper( 44 | const CameraParameters& cameraParametersLeft, 45 | const CameraParameters& cameraParametersRight, 46 | const double stereo_baseline, 47 | const RowMatcher& rowMatcher, 48 | const Parameters& params, 49 | std::shared_ptr motionModel, 50 | const size_t imageBeginIndex 51 | ); 52 | 53 | #ifdef USE_LOOPCLOSURE 54 | void setLoopClosing(std::unique_ptr& loop_detector); 55 | #endif 56 | 57 | virtual void Add(const size_t frame_id, const ros::Time& time, std::unique_ptr stereoImageFeatures); 58 | 59 | inline sptam::Map& GetMap() 60 | { return sptam_.GetMap(); } 61 | 62 | virtual void Stop() 63 | { sptam_.stop(); } 64 | 65 | protected: 66 | 67 | const CameraParameters cameraParametersLeft_; 68 | const CameraParameters cameraParametersRight_; 69 | const double stereo_baseline_; 70 | RowMatcher rowMatcher_; 71 | Parameters params_; 72 | std::shared_ptr motionModel_; 73 | bool isMapInitialized_; 74 | SPTAM sptam_; 75 | }; 76 | -------------------------------------------------------------------------------- /src/standAlone/StereoImageFeatures.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include "../sptam/StereoFrame.hpp" 36 | 37 | struct StereoImageFeatures 38 | { 39 | StereoImageFeatures( 40 | const ImageFeatures& imgFeaturesLeft, 41 | const ImageFeatures& imgFeaturesRight, 42 | const cv::Mat& imgLeft, 43 | const cv::Mat& imgRight) 44 | : imageFeaturesLeft( imgFeaturesLeft ) 45 | , imageFeaturesRight( imgFeaturesRight ) 46 | { 47 | imageLeft = imgLeft.clone(); 48 | imageRight = imgRight.clone(); 49 | } 50 | 51 | ImageFeatures imageFeaturesLeft; 52 | ImageFeatures imageFeaturesRight; 53 | 54 | cv::Mat imageLeft; 55 | cv::Mat imageRight; 56 | }; 57 | -------------------------------------------------------------------------------- /src/standAlone/Timestamps.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | 39 | // ros::Time 40 | #ifdef BUILDING_ROS 41 | #include 42 | #else 43 | #include "../sptam/utils/time/time.h" 44 | #endif 45 | 46 | class Timestamps 47 | { 48 | public: 49 | 50 | /** 51 | * @brief create a timestamp generator with a constant rate. 52 | */ 53 | Timestamps(const double rate, size_t frame_ini = 0); 54 | 55 | /** 56 | * @brief create a timestamp generator froma a list of timestamps. 57 | */ 58 | Timestamps(const std::string& filename, size_t frame_ini = 0); 59 | 60 | /** 61 | * @brief Get the next timestamp when it becomes avaible. 62 | * This method sleeps for any leftover time since the last time it was called. 63 | */ 64 | ros::Time getNextWhenReady(); 65 | 66 | private: 67 | 68 | bool constant_rate_; 69 | 70 | double rate_; 71 | ros::Time next_time_; 72 | 73 | size_t next_frame_; 74 | std::vector times_; 75 | 76 | ros::Time last_time_; 77 | ros::Time last_time_update_; 78 | }; 79 | -------------------------------------------------------------------------------- /src/standAlone/utils/ProgramOptions.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "ProgramOptions.hpp" 35 | 36 | ProgramOptions::ProgramOptions(const std::string& app_name) 37 | : app_name_( app_name ), n_optional_arguments_(0), n_positional_arguments_(0) 38 | {} 39 | 40 | 41 | // addOptionalArgument con aridad para optional_arguments no se necesitan un argumento. Ej: --help 42 | void ProgramOptions::addOptionalArgumentFlag(const std::string& name, const std::string& description) 43 | { 44 | optional_arguments_.add_options() 45 | (name.c_str(), description.c_str()); 46 | 47 | n_optional_arguments_++; 48 | } 49 | 50 | void ProgramOptions::parse(int argc, char **argv) 51 | { 52 | boost::program_options::options_description all_options; 53 | all_options.add( optional_arguments_ ); 54 | all_options.add( hidden_arguments_ ); 55 | 56 | // throws on error 57 | boost::program_options::store( 58 | boost::program_options::command_line_parser(argc, argv) 59 | .options( all_options ) 60 | .positional( positional_arguments_ ) 61 | .run() 62 | , vm_); 63 | } 64 | 65 | void ProgramOptions::notify() 66 | { 67 | boost::program_options::notify( vm_ ); 68 | } 69 | 70 | int ProgramOptions::count(const std::string& name) const 71 | { 72 | return vm_.count( name ); 73 | } 74 | 75 | std::ostream& operator << ( std::ostream& os, const ProgramOptions& program_options ) 76 | { 77 | os << "Usage: " << program_options.app_name_ << " [OPTION]... "; 78 | 79 | for (size_t i=0; i 0 ) 89 | { 90 | os << "options" << std::endl << std::endl; 91 | 92 | os << /*program_options.hidden_arguments_ << */program_options.optional_arguments_; 93 | } 94 | 95 | return os; 96 | } 97 | -------------------------------------------------------------------------------- /src/standAlone/utils/ProgramOptions.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | 39 | class ProgramOptions 40 | { 41 | public: 42 | 43 | ProgramOptions(const std::string& app_name); 44 | 45 | template 46 | void addOptionalArgument(const std::string& name, const std::string& description, T& value); 47 | 48 | template 49 | void addPositionalArgument(const std::string& name, const std::string& description, T& value); 50 | 51 | void addOptionalArgumentFlag(const std::string& name, const std::string& description); 52 | 53 | /** 54 | * @brief parse command line values. 55 | */ 56 | void parse(int argc, char **argv); 57 | 58 | /** 59 | * @brief throws argument parsing exceptions. 60 | */ 61 | void notify(); 62 | 63 | /** 64 | * @brief count given values for an argument. 65 | */ 66 | int count(const std::string& name) const; 67 | 68 | /** 69 | * @brief: 70 | * Print usage message to stream. 71 | */ 72 | friend std::ostream& operator << ( std::ostream& os, const ProgramOptions& program_options ); 73 | 74 | private: 75 | 76 | std::string app_name_; 77 | 78 | size_t n_optional_arguments_; 79 | boost::program_options::options_description optional_arguments_; 80 | 81 | size_t n_positional_arguments_; 82 | boost::program_options::options_description hidden_arguments_; 83 | boost::program_options::positional_options_description positional_arguments_; 84 | 85 | boost::program_options::variables_map vm_; 86 | }; 87 | 88 | // positional arguments son los argumentos del programa que no tienen una bandera (--bandera argumento) que los anuncia. 89 | 90 | template 91 | void ProgramOptions::addPositionalArgument(const std::string& name, const std::string& description, T& value) 92 | { 93 | hidden_arguments_.add_options() 94 | (name.c_str(), boost::program_options::value(&value)->required(), description.c_str()); 95 | 96 | // TODO: number of expected values is hardcoded to 1 97 | positional_arguments_.add(name.c_str(), 1); 98 | 99 | n_positional_arguments_++; 100 | } 101 | 102 | // optional_arguments son aquellos que necesitan una bandera. Ej: --grnd-poses path_to_ground_truth 103 | 104 | template 105 | void ProgramOptions::addOptionalArgument(const std::string& name, const std::string& description, T& value ) 106 | { 107 | optional_arguments_.add_options() 108 | (name.c_str(), boost::program_options::value(&value), description.c_str()); 109 | 110 | n_optional_arguments_++; 111 | } 112 | -------------------------------------------------------------------------------- /src/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project( sptam ) 3 | 4 | find_package(GTest REQUIRED) 5 | 6 | add_subdirectory( sptam ) 7 | -------------------------------------------------------------------------------- /src/tests/sptam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project( sptam ) 3 | 4 | add_subdirectory( utils ) 5 | add_subdirectory( CovisibilityGraph ) 6 | -------------------------------------------------------------------------------- /src/tests/sptam/CovisibilityGraph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project( sptam ) 3 | 4 | find_package(Boost COMPONENTS system REQUIRED) 5 | 6 | include_directories(${SPTAM_INCLUDE_DIRS}) 7 | 8 | # Add test cpp file 9 | add_executable(test_CovisibilityGraph test_CovisibilityGraph.cpp ${SPTAM_SOURCE_DIRS}/utils/CovisibilityGraph.cpp) 10 | 11 | # Link test executable against gtest & gtest_main 12 | target_link_libraries(test_CovisibilityGraph gtest gtest_main ${Boost_LIBRARIES}) 13 | 14 | add_test( 15 | NAME test_CovisibilityGraph 16 | COMMAND test_CovisibilityGraph 17 | ) 18 | -------------------------------------------------------------------------------- /src/tests/sptam/CovisibilityGraph/test_CovisibilityGraph.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #include "gtest/gtest.h" 34 | #include "utils/CovisibilityGraph.hpp" 35 | #include "utils/CovisibilityGraph.cpp" 36 | 37 | typedef struct { 38 | size_t id; 39 | } KeyFrame; 40 | 41 | typedef struct { 42 | size_t id; 43 | } MapPoint; 44 | 45 | typedef struct { 46 | size_t id; 47 | } Measurement; 48 | 49 | typedef CovisibilityGraph SimpleGraph; 50 | 51 | TEST (BasicTest, Instantiation) 52 | { 53 | SimpleGraph graph; 54 | } 55 | 56 | TEST (BasicTest, Empty) 57 | { 58 | SimpleGraph graph; 59 | 60 | const SimpleGraph::SharedKeyFrameList& kfs = graph.getKeyframes(); 61 | EXPECT_EQ (0, kfs.size()); 62 | 63 | const SimpleGraph::SharedMapPointList& pts = graph.getMapPoints(); 64 | EXPECT_EQ (0, pts.size()); 65 | } 66 | 67 | TEST (BasicTest, addKeyFrame) 68 | { 69 | SimpleGraph graph; 70 | 71 | KeyFrame kf({1u}); 72 | graph.addKeyFrame( kf ); 73 | } 74 | 75 | TEST (BasicTest, addMapPoint) 76 | { 77 | SimpleGraph graph; 78 | 79 | KeyFrame kf({1u}); 80 | graph.addKeyFrame( kf ); 81 | } 82 | -------------------------------------------------------------------------------- /src/tests/sptam/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project( sptam ) 3 | 4 | add_subdirectory( set_union ) 5 | -------------------------------------------------------------------------------- /src/tests/sptam/utils/set_union/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project( sptam ) 3 | 4 | include_directories(${SPTAM_INCLUDE_DIRS}) 5 | 6 | # Add test cpp file 7 | add_executable(test_set_union test_set_union.cpp) 8 | target_link_libraries(test_set_union gtest gtest_main) 9 | 10 | add_test( 11 | NAME test_set_union 12 | COMMAND test_set_union 13 | ) 14 | -------------------------------------------------------------------------------- /src/tests/sptam/utils/set_union/test_set_union.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #include 34 | #include "gtest/gtest.h" 35 | #include "utils/set_union.hpp" 36 | 37 | TEST (SetUnion, BothEmpty) 38 | { 39 | std::set s1, s2; 40 | 41 | std::set u = setUnion(s1, s2); 42 | 43 | ASSERT_EQ(0, u.size()); 44 | } 45 | 46 | TEST (SetUnion, FirstEmpty) 47 | { 48 | std::set s1, s2; 49 | s2.insert( 2 ); 50 | 51 | std::set u = setUnion(s1, s2); 52 | 53 | ASSERT_EQ(1, u.size()); 54 | ASSERT_EQ(2, *(u.begin())); 55 | } 56 | 57 | TEST (SetUnion, SecondEmpty) 58 | { 59 | std::set s1, s2; 60 | s1.insert( 2 ); 61 | 62 | std::set u = setUnion(s1, s2); 63 | 64 | ASSERT_EQ(1, u.size()); 65 | ASSERT_EQ(2, *(u.begin())); 66 | } 67 | 68 | TEST (SetUnion, EqualSingletons) 69 | { 70 | std::set s1, s2; 71 | s1.insert( 2 ); 72 | s2.insert( 2 ); 73 | 74 | std::set u = setUnion(s1, s2); 75 | 76 | ASSERT_EQ(1, u.size()); 77 | ASSERT_EQ(2, *(u.begin())); 78 | } 79 | 80 | TEST (SetUnion, Disjunct) 81 | { 82 | std::set s1, s2; 83 | s1.insert( 1 ); s1.insert( 3 ); 84 | s2.insert( 2 ); s2.insert( 4 ); 85 | 86 | std::set u = setUnion(s1, s2); 87 | 88 | ASSERT_EQ(4, u.size()); 89 | 90 | std::list expected; 91 | for(int i=1; i<1+4; i++) 92 | expected.push_back( i ); 93 | 94 | ASSERT_TRUE( std::equal(u.begin(), u.end(), expected.begin()) ); 95 | } 96 | 97 | TEST (SetUnion, Overlapping) 98 | { 99 | std::set s1, s2; 100 | s1.insert( 1 ); s1.insert( 2 ); 101 | s2.insert( 2 ); s2.insert( 3 ); 102 | 103 | std::set u = setUnion(s1, s2); 104 | 105 | ASSERT_EQ(3, u.size()); 106 | 107 | std::list expected; 108 | for(int i=1; i<1+3; i++) 109 | expected.push_back( i ); 110 | 111 | ASSERT_TRUE( std::equal(u.begin(), u.end(), expected.begin()) ); 112 | } 113 | --------------------------------------------------------------------------------