├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── configurations ├── configuration_euroc.yaml ├── configuration_kitti.yaml └── configuration_kitti_fast.yaml ├── contributors.txt ├── executables ├── CMakeLists.txt ├── README.md ├── app.cpp ├── node.cpp ├── stereo_calibrator.cpp ├── test_stereo_frontend.cpp ├── trajectory_analyzer.cpp └── trajectory_converter.cpp ├── package.xml ├── pull_srrg_packages.bash └── src ├── CMakeLists.txt ├── aligners ├── CMakeLists.txt ├── base_aligner.h ├── base_frame_aligner.h ├── base_local_map_aligner.h ├── stereouv_aligner.cpp ├── stereouv_aligner.h ├── uvd_aligner.cpp ├── uvd_aligner.h ├── xyz_aligner.cpp └── xyz_aligner.h ├── framepoint_generation ├── CMakeLists.txt ├── base_framepoint_generator.cpp ├── base_framepoint_generator.h ├── depth_framepoint_generator.cpp ├── depth_framepoint_generator.h ├── intensity_feature_extractor.h ├── intensity_feature_matcher.cpp ├── intensity_feature_matcher.h ├── stereo_framepoint_generator.cpp └── stereo_framepoint_generator.h ├── map_optimization ├── CMakeLists.txt ├── graph_optimizer.cpp └── graph_optimizer.h ├── position_tracking ├── CMakeLists.txt ├── base_tracker.cpp ├── base_tracker.h ├── depth_tracker.cpp ├── depth_tracker.h ├── stereo_tracker.cpp └── stereo_tracker.h ├── relocalization ├── CMakeLists.txt ├── closure.h ├── relocalizer.cpp └── relocalizer.h ├── system ├── CMakeLists.txt ├── slam_assembly.cpp └── slam_assembly.h ├── types ├── CMakeLists.txt ├── camera.cpp ├── camera.h ├── definitions.h ├── frame.cpp ├── frame.h ├── frame_point.cpp ├── frame_point.h ├── landmark.cpp ├── landmark.h ├── local_map.cpp ├── local_map.h ├── parameters.cpp ├── parameters.h ├── world_map.cpp └── world_map.h └── visualization ├── CMakeLists.txt ├── image_viewer.cpp ├── image_viewer.h ├── map_viewer.cpp └── map_viewer.h /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | #* -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(srrg_proslam) 3 | 4 | find_package(srrg_cmake_modules REQUIRED) 5 | set(CMAKE_MODULE_PATH ${srrg_cmake_modules_INCLUDE_DIRS}) 6 | 7 | #ds determine build type, default build type: release 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE RELEASE) 10 | endif() 11 | message("${PROJECT_NAME}|build type: '${CMAKE_BUILD_TYPE}'") 12 | 13 | #ds flags for release build 14 | if("${CMAKE_BUILD_TYPE}" STREQUAL "Release" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RELEASE") 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -Wall -pedantic -Ofast -DNDEBUG") 16 | message("${PROJECT_NAME}|adding flags for '${CMAKE_BUILD_TYPE}': '-std=c++11 -march=native -Wall -pedantic -Ofast -DNDEBUG'") 17 | 18 | #ds flags for other build(s) (e.g. debug) 19 | else() 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native -Wall -pedantic -O0 -g -fstack-check") 21 | message("${PROJECT_NAME}|adding flags for '${CMAKE_BUILD_TYPE}': '-std=c++11 -march=native -Wall -pedantic -O0 -g -fstack-check'") 22 | endif() 23 | 24 | #ds enable ARM flags if applicable 25 | if("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l") 26 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=armv7-a -mfpu=neon-vfpv4 -mfloat-abi=hard -funsafe-math-optimizations") 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mfpu=neon-vfpv4 -mfloat-abi=hard -funsafe-math-optimizations") 28 | message("${PROJECT_NAME}|enabling ARM neon optimizations") 29 | endif() 30 | 31 | #ds specify target binary descriptor bit size (256 if not defined) 32 | add_definitions(-DSRRG_PROSLAM_DESCRIPTOR_SIZE_BITS=256) 33 | 34 | #ds specify target log level: 0 ERROR, 1 WARNING, 2 INFO, 3 DEBUG (defaults to 2 if not defined) 35 | add_definitions(-DSRRG_PROSLAM_LOG_LEVEL=2) 36 | 37 | #ds enable descriptor merging in HBST (and other SRRG components) - careful for collisions with landmark merging! 38 | add_definitions(-DSRRG_MERGE_DESCRIPTORS) 39 | 40 | #ds load Eigen library 41 | find_package(Eigen3 REQUIRED) 42 | message("${PROJECT_NAME}|using Eigen version: '3' (${EIGEN3_INCLUDE_DIR})") 43 | 44 | #ds enable Eigen for HBST 45 | add_definitions(-DSRRG_HBST_HAS_EIGEN) 46 | 47 | #ds check if a supported ros version is installed to determine which packages we include 48 | set(SRRG_PROSLAM_HAS_ROS false) 49 | if("$ENV{ROS_DISTRO}" STREQUAL "kinetic" OR "$ENV{ROS_DISTRO}" STREQUAL "indigo" OR "$ENV{ROS_DISTRO}" STREQUAL "melodic") 50 | 51 | #ds ROS support enabled 52 | message("${PROJECT_NAME}|using ROS version: '$ENV{ROS_DISTRO}' (building nodes)") 53 | set(SRRG_PROSLAM_HAS_ROS true) 54 | find_package(catkin REQUIRED COMPONENTS 55 | srrg_core 56 | srrg_gl_helpers 57 | srrg_core_viewers 58 | srrg_hbst 59 | roscpp 60 | sensor_msgs 61 | cv_bridge 62 | nav_msgs 63 | message_filters 64 | image_geometry 65 | ) 66 | 67 | #ds switch to ROS opencv libraries without calling find_package to avoid conflicts with other existing OpenCV installations 68 | set(OpenCV_DIR "ROS") 69 | set(OpenCV_LIBS ${catkin_LIBRARIES}) 70 | message("${PROJECT_NAME}|using ROS OpenCV version: '${OpenCV_VERSION}' (${OpenCV_DIR})") 71 | add_definitions(-DSRRG_HBST_HAS_OPENCV) 72 | else() 73 | 74 | #ds build proslam without ROS components 75 | find_package(catkin REQUIRED COMPONENTS 76 | srrg_core 77 | srrg_gl_helpers 78 | srrg_core_viewers 79 | srrg_hbst 80 | ) 81 | 82 | #ds OpenCV - OpenCV_DIR might be overwritten by user 83 | find_package(OpenCV REQUIRED) 84 | message("${PROJECT_NAME}|using OpenCV version: '${OpenCV_VERSION}' (${OpenCV_DIR})") 85 | add_definitions(-DSRRG_HBST_HAS_OPENCV) 86 | endif() 87 | 88 | #ds check for OpenCV contrib library (optional) 89 | string(FIND "${OpenCV_LIBS}" "xfeatures2d" FOUND_OPENCV_CONTRIB) 90 | if(NOT ${FOUND_OPENCV_CONTRIB} EQUAL -1) 91 | message("${PROJECT_NAME}|found xfeatures2d library, building contributed OpenCV components") 92 | add_definitions(-DSRRG_PROSLAM_HAS_OPENCV_CONTRIB) 93 | else() 94 | message("${PROJECT_NAME}|xfeatures2d library not found, using ORB instead of BRIEF descriptors") 95 | endif() 96 | 97 | #ds load QGLViewer library 98 | find_package(QGLViewer REQUIRED) 99 | 100 | #ds attempt to locate a g2o package through cmake modules 101 | find_package(G2O QUIET) 102 | 103 | #ds set ownership model for g2o TODO retrieve this information from g2o (current g2o requires cmake 3+) 104 | if (${CMAKE_MAJOR_VERSION} GREATER 2) 105 | 106 | #ds (comment this line if you're using an older g2o version but cmake 3+) 107 | add_definitions(-DSRRG_PROSLAM_G2O_HAS_NEW_OWNERSHIP_MODEL) 108 | message("${PROJECT_NAME}|found CMake 3+: assuming new g2o ownership model") 109 | else() 110 | 111 | #ds old g2o ownership model is used 112 | message("${PROJECT_NAME}|found CMake 2: assuming old g2o ownership model") 113 | endif() 114 | 115 | #ds check if a custom g2o library is installed 116 | set(SRRG_PROSLAM_HAS_OWN_G2O false) 117 | 118 | #ds if theres no SRRG g2o installation 119 | if("${G2O_SRRG_DIR}" STREQUAL "") 120 | 121 | #ds check if theres also no srrg g2o installation 122 | if("$ENV{G2O_ROOT}" STREQUAL "") 123 | 124 | #ds no custom g2o installation found, fallback to catkin g2o 125 | message("${PROJECT_NAME}|using catkin g2o") 126 | else() 127 | 128 | #ds use custom g2o 129 | message("${PROJECT_NAME}|using custom g2o: '$ENV{G2O_ROOT}'") 130 | set(SRRG_PROSLAM_HAS_OWN_G2O true) 131 | endif() 132 | else() 133 | 134 | #ds use srrg g2o 135 | message("${PROJECT_NAME}|using SRRG g2o: '${G2O_SRRG_DIR}'") 136 | set(SRRG_PROSLAM_HAS_OWN_G2O true) 137 | endif() 138 | 139 | #ds if a custom g2o package was found 140 | if(SRRG_PROSLAM_HAS_OWN_G2O) 141 | 142 | #ds add it to our variables 143 | set(g2o_INCLUDE_DIRS ${G2O_INCLUDE_DIR}) 144 | set(g2o_LIBRARIES ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SLAM3D} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY}) 145 | 146 | else() 147 | 148 | #ds attempt to find a catkin g2o 149 | find_package(g2o_catkin REQUIRED) 150 | 151 | #ds add it to our variables 152 | set(g2o_INCLUDE_DIRS ${g2o_catkin_INCLUDE_DIRS}) 153 | set(g2o_LIBRARIES ${g2o_catkin_LIBRARIES}) 154 | endif() 155 | 156 | #ds load suite sparse for pose graph optimization 157 | find_package(SuiteSparse REQUIRED) 158 | 159 | #ds specify additional locations of header files 160 | #ds treating them as system includes to surpress warnings (!) 161 | include_directories(SYSTEM 162 | ${EIGEN3_INCLUDE_DIR} 163 | ${g2o_INCLUDE_DIRS} 164 | ${CSPARSE_INCLUDE_DIR} 165 | ${OpenCV_INCLUDE_DIRS} 166 | ${QGLVIEWER_INCLUDE_DIR} 167 | ${catkin_INCLUDE_DIRS} 168 | ${SRRG_QT_INCLUDE_DIRS} 169 | src 170 | ) 171 | 172 | #ds help the catkin tool on 16.04 (cmake seems unable to find single libraries, although catkin claims the link_directories call is not required) 173 | #ds in order to avoid linking against the catkin_LIBRARIES bulk everytime enable this so one can select single libraries 174 | link_directories(${catkin_LIBRARY_DIRS}) 175 | 176 | #ds set up catkin package (exported components) 177 | catkin_package( 178 | INCLUDE_DIRS 179 | ${EIGEN3_INCLUDE_DIR} 180 | ${g2o_INCLUDE_DIRS} 181 | ${CSPARSE_INCLUDE_DIR} 182 | ${OpenCV_INCLUDE_DIRS} 183 | ${QGLVIEWER_INCLUDE_DIR} 184 | ${catkin_INCLUDE_DIRS} 185 | ${SRRG_QT_INCLUDE_DIRS} 186 | src 187 | 188 | LIBRARIES 189 | 190 | #ds package libraries 191 | srrg_proslam_aligners_library 192 | srrg_proslam_framepoint_generation_library 193 | srrg_proslam_map_optimization_library 194 | srrg_proslam_position_tracking_library 195 | srrg_proslam_relocalization_library 196 | srrg_proslam_types_library 197 | srrg_proslam_visualization_library 198 | srrg_proslam_slam_assembly_library 199 | 200 | #ds export the used libraries in this project (consistency for integration) 201 | ${g2o_LIBRARIES} 202 | ${CSPARSE_LIBRARY} 203 | ${OpenCV_LIBS} 204 | yaml-cpp 205 | ${OPENGL_gl_LIBRARY} 206 | ${OPENGL_glu_LIBRARY} 207 | ${QGLVIEWER_LIBRARY} 208 | ${SRRG_QT_LIBRARIES} 209 | ) 210 | 211 | #ds set sources 212 | add_subdirectory(src) 213 | add_subdirectory(executables) 214 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, srrg-software 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /configurations/configuration_euroc.yaml: -------------------------------------------------------------------------------- 1 | command_line: 2 | 3 | #TODO THIS FILE NEEDS TO BE UPDATED! 4 | 5 | #system log level (select one: DEBUG, INFO, WARNING, ERROR) 6 | logging_level: DEBUG 7 | 8 | #ds processing mode (select one: RGB_STEREO, RGB_DEPTH) 9 | tracker_mode: RGB_STEREO 10 | 11 | #ds topic names 12 | topic_image_left: /camera_left/image_raw 13 | topic_image_right: /camera_right/image_raw 14 | topic_camera_info_left: /camera_left/camera_info 15 | topic_camera_info_right: /camera_right/camera_info 16 | 17 | #ds dataset file name 18 | dataset_file_name: 19 | 20 | #ds options 21 | option_use_gui: false 22 | option_use_odometry: false 23 | option_disable_relocalization: false 24 | option_show_top_viewer: false 25 | option_drop_framepoints: false 26 | option_equalize_histogram: false 27 | option_recover_landmarks: true 28 | option_disable_bundle_adjustment: true 29 | option_save_pose_graph: false 30 | 31 | landmark: 32 | 33 | #ds minimum number of measurements to always integrate 34 | minimum_number_of_forced_updates: 2 35 | 36 | local_map: 37 | 38 | #ds target minimum number of landmarks for local map creation 39 | minimum_number_of_landmarks: 100 40 | 41 | world_map: 42 | 43 | #ds key frame generation properties 44 | minimum_distance_traveled_for_local_map: 1.0 45 | minimum_degrees_rotated_for_local_map: 0.5 46 | minimum_number_of_frames_for_local_map: 10 47 | 48 | base_framepoint_generation: 49 | 50 | #ds feature descriptor type (BRIEF-128/256/512, ORB-256, BRISK-512, FREAK-512, A-KAZE-486, BinBoost-064) 51 | descriptor_type: ORB-256 52 | 53 | #ds dynamic thresholds for feature detection 54 | target_number_of_keypoints_tolerance: 0.1 55 | detector_threshold_maximum_change: 1.0 56 | detector_threshold_initial: 15 57 | detector_threshold_minimum: 15 58 | detector_threshold_maximum: 100 59 | number_of_detectors_vertical: 1 60 | number_of_detectors_horizontal: 1 61 | 62 | #ds dynamic thresholds for descriptor matching 63 | matching_distance_tracking_threshold: 35 64 | 65 | #ds maximum reliable depth with chosen sensor 66 | maximum_reliable_depth_meters: 5.0 67 | 68 | #feature density regularization 69 | enable_keypoint_binning: true 70 | bin_size_pixels: 10 71 | 72 | stereo_framepoint_generation: 73 | 74 | #ds stereo: triangulation 75 | maximum_matching_distance_triangulation: 50 76 | minimum_disparity_pixels: 1 77 | maximum_epipolar_search_offset_pixels: 0 78 | 79 | depth_framepoint_generation: 80 | 81 | #ds depth sensor configuration 82 | maximum_depth_near_meters: 5 83 | maximum_depth_far_meters: 20 84 | 85 | base_tracking: 86 | 87 | #ds this criteria is used for the decision of whether creating a landmark or not from a track of framepoints 88 | minimum_track_length_for_landmark_creation: 2 89 | 90 | #ds track lost criteria 91 | minimum_number_of_landmarks_to_track: 5 92 | 93 | #point tracking thresholds 94 | minimum_projection_tracking_distance_pixels: 10 95 | maximum_projection_tracking_distance_pixels: 50 96 | maximum_distance_tracking_pixels: 22500 #150x150 maximum allowed pixel distance between image coordinates prediction and actual detection 97 | range_point_tracking: 2 #pixel search range width for point vicinity tracking 98 | 99 | #landmark track recovery (if enabled) 100 | maximum_number_of_landmark_recoveries: 10 101 | 102 | #ds motion model for initial pose guess (select one: NONE, CONSTANT_VELOCITY, CAMERA_ODOMETRY) 103 | motion_model: CONSTANT_VELOCITY 104 | 105 | #pose optimization 106 | minimum_delta_angular_for_movement: 0.001 107 | minimum_delta_translational_for_movement: 0.01 108 | 109 | #pose optimization: aligner unit configuration 110 | aligner->error_delta_for_convergence: 1e-3 111 | aligner->maximum_error_kernel: 9 112 | aligner->damping: 0 113 | aligner->maximum_number_of_iterations: 1000 114 | aligner->minimum_number_of_inliers: 0 115 | aligner->minimum_inlier_ratio: 0 116 | 117 | relocalization: 118 | 119 | #minimum query interspace 120 | preliminary_minimum_interspace_queries: 10 121 | 122 | #minimum relative number of matches 123 | preliminary_minimum_matching_ratio: 0.2 124 | 125 | #minimum absolute number of matches 126 | minimum_number_of_matches_per_landmark: 5 127 | 128 | #correspondence retrieval 129 | minimum_matches_per_correspondence: 0 130 | 131 | #icp: aligner unit configuration 132 | aligner->error_delta_for_convergence: 1e-5 133 | aligner->maximum_error_kernel: 0.25 134 | aligner->damping: 0.0 135 | aligner->maximum_number_of_iterations: 1000 136 | aligner->minimum_number_of_inliers: 50 137 | aligner->minimum_inlier_ratio: 0.25 138 | 139 | graph_optimization: 140 | 141 | #enable full bundle adjustment (default: only pose graph optimization upon loop closing) 142 | enable_full_bundle_adjustment: false 143 | 144 | #g2o factor graph optimization algorithm: GAUSS_NEWTON, LEVENBERG 145 | optimization_algorithm: GAUSS_NEWTON 146 | 147 | #g2o linear solver type to perform optimization algorithm: CHOLMOD, CSPARSE 148 | linear_solver_type: CHOLMOD 149 | 150 | #g2o identifier space between frames and landmark vertices 151 | identifier_space: 1e6 152 | 153 | #maximum number of iterations graph optimization 154 | maximum_number_of_iterations: 100 155 | 156 | #determines window size for bundle adjustment 157 | number_of_frames_per_bundle_adjustment: 100 158 | 159 | #base frame weight in pose graph (assuming 1 for landmarks) 160 | base_information_frame: 1e4 161 | 162 | #free translation for pose to pose measurements 163 | free_translation_for_poses: true 164 | 165 | #translational frame weight reduction in pose graph 166 | base_information_frame_factor_for_translation: 1e-4 167 | 168 | #enable robust kernel for loop closure measurements 169 | enable_robust_kernel_for_poses: true 170 | 171 | #enable robust kernel for landmark measurements 172 | enable_robust_kernel_for_landmarks: false 173 | 174 | visualization: 175 | -------------------------------------------------------------------------------- /configurations/configuration_kitti.yaml: -------------------------------------------------------------------------------- 1 | command_line: 2 | 3 | #system log level (select one: DEBUG, INFO, WARNING, ERROR) 4 | logging_level: DEBUG 5 | 6 | #ds processing mode (select one: RGB_STEREO, RGB_DEPTH) 7 | tracker_mode: RGB_STEREO 8 | 9 | #ds topic names 10 | topic_image_left: /camera_left/image_raw 11 | topic_image_right: /camera_right/image_raw 12 | topic_camera_info_left: /camera_left/camera_info 13 | topic_camera_info_right: /camera_right/camera_info 14 | 15 | #ds dataset file name 16 | dataset_file_name: 17 | 18 | #ds options 19 | option_use_gui: false 20 | option_use_odometry: false 21 | option_disable_relocalization: false 22 | option_show_top_viewer: false 23 | option_drop_framepoints: false 24 | option_equalize_histogram: false 25 | option_recover_landmarks: true 26 | option_disable_bundle_adjustment: true 27 | option_save_pose_graph: false 28 | 29 | landmark: 30 | 31 | #ds minimum number of measurements to always integrate 32 | minimum_number_of_forced_updates: 2 33 | 34 | local_map: 35 | 36 | #ds target minimum number of landmarks for local map creation 37 | minimum_number_of_landmarks: 50 38 | 39 | #ds target maximum number of landmarks for local map creation 40 | maximum_number_of_landmarks: 1000 41 | 42 | world_map: 43 | 44 | #ds key frame generation properties 45 | minimum_distance_traveled_for_local_map: 1.0 46 | minimum_degrees_rotated_for_local_map: 0.5 47 | minimum_number_of_frames_for_local_map: 4 48 | 49 | base_framepoint_generation: 50 | 51 | #ds feature descriptor type (BRIEF-128/256/512, ORB-256, BRISK-512, FREAK-512, A-KAZE-486, BinBoost-064) 52 | descriptor_type: BRIEF-256 53 | 54 | #ds dynamic thresholds for feature detection 55 | target_number_of_keypoints_tolerance: 0.1 56 | detector_threshold_maximum_change: 0.2 57 | detector_threshold_minimum: 20 58 | detector_threshold_maximum: 100 59 | number_of_detectors_vertical: 3 60 | number_of_detectors_horizontal: 3 61 | 62 | #point tracking thresholds 63 | minimum_projection_tracking_distance_pixels: 15 64 | maximum_projection_tracking_distance_pixels: 50 65 | 66 | #ds dynamic thresholds for descriptor matching 67 | matching_distance_tracking_threshold: 35 68 | 69 | #ds maximum reliable depth with chosen sensor 70 | maximum_reliable_depth_meters: 15.0 71 | 72 | #feature density regularization 73 | enable_keypoint_binning: true 74 | bin_size_pixels: 14 75 | 76 | stereo_framepoint_generation: 77 | 78 | #ds stereo: triangulation 79 | maximum_matching_distance_triangulation: 95 80 | minimum_disparity_pixels: 1 81 | maximum_epipolar_search_offset_pixels: 0 82 | 83 | depth_framepoint_generation: 84 | 85 | #ds depth sensor configuration 86 | maximum_depth_near_meters: 5 87 | maximum_depth_far_meters: 20 88 | 89 | base_tracking: 90 | 91 | #ds this criteria is used for the decision of whether creating a landmark or not from a track of framepoints 92 | minimum_track_length_for_landmark_creation: 2 93 | 94 | #ds track lost criteria 95 | minimum_number_of_landmarks_to_track: 5 96 | 97 | #point tracking thresholds 98 | tunnel_vision_ratio: 0.75 99 | 100 | #landmark track recovery (if enabled) 101 | maximum_number_of_landmark_recoveries: 10 102 | 103 | #ds motion model for initial pose guess (select one: NONE, CONSTANT_VELOCITY, CAMERA_ODOMETRY) 104 | motion_model: CONSTANT_VELOCITY 105 | 106 | #pose optimization 107 | minimum_delta_angular_for_movement: 0.001 108 | minimum_delta_translational_for_movement: 0.01 109 | 110 | #pose optimization: aligner unit configuration 111 | aligner->error_delta_for_convergence: 1e-3 112 | aligner->maximum_error_kernel: 16 113 | aligner->damping: 1000 114 | aligner->maximum_number_of_iterations: 1000 115 | aligner->minimum_number_of_inliers: 0 116 | aligner->minimum_inlier_ratio: 0 117 | 118 | relocalization: 119 | 120 | #maximum permitted descriptor distance to still be considered as a match 121 | maximum_descriptor_distance: 25 122 | 123 | #minimum query interspace 124 | preliminary_minimum_interspace_queries: 50 125 | 126 | #minimum relative number of matches 127 | preliminary_minimum_matching_ratio: 0.1 128 | 129 | #minimum absolute number of matches 130 | minimum_number_of_matched_landmarks: 25 131 | 132 | #correspondence retrieval 133 | minimum_matches_per_correspondence: 0 134 | 135 | #icp: aligner unit configuration 136 | aligner->error_delta_for_convergence: 1e-5 137 | aligner->maximum_error_kernel: 1.0 138 | aligner->damping: 0.0 139 | aligner->maximum_number_of_iterations: 100 140 | aligner->minimum_number_of_inliers: 25 141 | aligner->minimum_inlier_ratio: 0.5 142 | 143 | graph_optimization: 144 | 145 | #enable full bundle adjustment (default: only pose graph optimization upon loop closing) 146 | enable_full_bundle_adjustment: false 147 | 148 | #g2o factor graph optimization algorithm: GAUSS_NEWTON, LEVENBERG 149 | optimization_algorithm: GAUSS_NEWTON 150 | 151 | #g2o linear solver type to perform optimization algorithm: CHOLMOD, CSPARSE 152 | linear_solver_type: CHOLMOD 153 | 154 | #g2o identifier space between frames and landmark vertices 155 | identifier_space: 1e9 156 | 157 | #maximum number of iterations graph optimization 158 | maximum_number_of_iterations: 100 159 | 160 | #determines window size for bundle adjustment 161 | number_of_frames_per_bundle_adjustment: 100 162 | 163 | #base frame weight in pose graph (assuming 1 for landmarks) 164 | base_information_frame: 1e5 165 | 166 | #free translation for pose to pose measurements 167 | free_translation_for_poses: true 168 | 169 | #translational frame weight reduction in pose graph 170 | base_information_frame_factor_for_translation: 1e-5 171 | 172 | #enable robust kernel for loop closure measurements 173 | enable_robust_kernel_for_poses: true 174 | 175 | #enable robust kernel for landmark measurements 176 | enable_robust_kernel_for_landmarks: false 177 | 178 | visualization: 179 | -------------------------------------------------------------------------------- /configurations/configuration_kitti_fast.yaml: -------------------------------------------------------------------------------- 1 | command_line: 2 | 3 | #system log level (select one: DEBUG, INFO, WARNING, ERROR) 4 | logging_level: DEBUG 5 | 6 | #ds processing mode (select one: RGB_STEREO, RGB_DEPTH) 7 | tracker_mode: RGB_STEREO 8 | 9 | #ds topic names 10 | topic_image_left: /camera_left/image_raw 11 | topic_image_right: /camera_right/image_raw 12 | topic_camera_info_left: /camera_left/camera_info 13 | topic_camera_info_right: /camera_right/camera_info 14 | 15 | #ds dataset file name 16 | dataset_file_name: 17 | 18 | #ds options 19 | option_use_gui: false 20 | option_use_odometry: false 21 | option_disable_relocalization: false 22 | option_show_top_viewer: false 23 | option_drop_framepoints: false 24 | option_equalize_histogram: false 25 | option_recover_landmarks: false 26 | option_disable_bundle_adjustment: true 27 | option_save_pose_graph: false 28 | 29 | landmark: 30 | 31 | #ds minimum number of measurements to always integrate 32 | minimum_number_of_forced_updates: 2 33 | 34 | local_map: 35 | 36 | #ds target minimum number of landmarks for local map creation 37 | minimum_number_of_landmarks: 50 38 | 39 | world_map: 40 | 41 | #ds key frame generation properties 42 | minimum_distance_traveled_for_local_map: 0.5 43 | minimum_degrees_rotated_for_local_map: 0.5 44 | minimum_number_of_frames_for_local_map: 4 45 | 46 | base_framepoint_generation: 47 | 48 | #ds feature descriptor type (BRIEF-128/256/512, ORB-256, BRISK-512, FREAK-512, A-KAZE-486, BinBoost-064) 49 | descriptor_type: BRIEF-256 50 | 51 | #ds dynamic thresholds for feature detection 52 | target_number_of_keypoints_tolerance: 0.1 53 | detector_threshold_maximum_change: 0.5 54 | detector_threshold_initial: 15 55 | detector_threshold_minimum: 15 56 | detector_threshold_maximum: 100 57 | number_of_detectors_vertical: 1 58 | number_of_detectors_horizontal: 1 59 | 60 | #point tracking thresholds 61 | minimum_projection_tracking_distance_pixels: 10 62 | maximum_projection_tracking_distance_pixels: 50 63 | 64 | #ds dynamic thresholds for descriptor matching 65 | matching_distance_tracking_threshold: 40 66 | 67 | #ds maximum reliable depth with chosen sensor 68 | maximum_reliable_depth_meters: 15.0 69 | 70 | #feature density regularization 71 | enable_keypoint_binning: true 72 | bin_size_pixels: 25 73 | 74 | stereo_framepoint_generation: 75 | 76 | #ds stereo: triangulation 77 | maximum_matching_distance_triangulation: 60 78 | minimum_disparity_pixels: 1 79 | maximum_epipolar_search_offset_pixels: 0 80 | 81 | depth_framepoint_generation: 82 | 83 | #ds depth sensor configuration 84 | maximum_depth_near_meters: 5 85 | maximum_depth_far_meters: 20 86 | 87 | base_tracking: 88 | 89 | #ds this criteria is used for the decision of whether creating a landmark or not from a track of framepoints 90 | minimum_track_length_for_landmark_creation: 2 91 | 92 | #ds track lost criteria 93 | minimum_number_of_landmarks_to_track: 5 94 | 95 | #point tracking thresholds 96 | tunnel_vision_ratio: 0.75 97 | 98 | #landmark track recovery (if enabled) 99 | maximum_number_of_landmark_recoveries: 10 100 | 101 | #ds motion model for initial pose guess (select one: NONE, CONSTANT_VELOCITY, CAMERA_ODOMETRY) 102 | motion_model: CONSTANT_VELOCITY 103 | 104 | #pose optimization 105 | minimum_delta_angular_for_movement: 0.001 106 | minimum_delta_translational_for_movement: 0.01 107 | 108 | #pose optimization: aligner unit configuration 109 | aligner->error_delta_for_convergence: 1e-3 110 | aligner->maximum_error_kernel: 16 111 | aligner->damping: 0 112 | aligner->maximum_number_of_iterations: 1000 113 | aligner->minimum_number_of_inliers: 0 114 | aligner->minimum_inlier_ratio: 0 115 | 116 | relocalization: 117 | 118 | #minimum query interspace 119 | preliminary_minimum_interspace_queries: 50 120 | 121 | #minimum relative number of matches 122 | preliminary_minimum_matching_ratio: 0.3 123 | 124 | #minimum absolute number of matches 125 | minimum_number_of_matches_per_landmark: 10 126 | 127 | #correspondence retrieval 128 | minimum_matches_per_correspondence: 1 129 | 130 | #icp: aligner unit configuration 131 | aligner->error_delta_for_convergence: 1e-5 132 | aligner->maximum_error_kernel: 2.0 133 | aligner->damping: 0.0 134 | aligner->maximum_number_of_iterations: 100 135 | aligner->minimum_number_of_inliers: 10 136 | aligner->minimum_inlier_ratio: 0.5 137 | 138 | graph_optimization: 139 | 140 | #enable full bundle adjustment (default: only pose graph optimization upon loop closing) 141 | enable_full_bundle_adjustment: false 142 | 143 | #g2o factor graph optimization algorithm: GAUSS_NEWTON, LEVENBERG 144 | optimization_algorithm: GAUSS_NEWTON 145 | 146 | #g2o linear solver type to perform optimization algorithm: CHOLMOD, CSPARSE 147 | linear_solver_type: CHOLMOD 148 | 149 | #g2o identifier space between frames and landmark vertices 150 | identifier_space: 1e6 151 | 152 | #maximum number of iterations graph optimization 153 | maximum_number_of_iterations: 100 154 | 155 | #determines window size for bundle adjustment 156 | number_of_frames_per_bundle_adjustment: 100 157 | 158 | #base frame weight in pose graph (assuming 1 for landmarks) 159 | base_information_frame: 1e4 160 | 161 | #free translation for pose to pose measurements 162 | free_translation_for_poses: true 163 | 164 | #translational frame weight reduction in pose graph 165 | base_information_frame_factor_for_translation: 1e-4 166 | 167 | #enable robust kernel for loop closure measurements 168 | enable_robust_kernel_for_poses: true 169 | 170 | #enable robust kernel for landmark measurements 171 | enable_robust_kernel_for_landmarks: false 172 | 173 | visualization: 174 | -------------------------------------------------------------------------------- /contributors.txt: -------------------------------------------------------------------------------- 1 | Dominik Schlegel 2 | Mirco Colosi 3 | Giorgio Grisetti 4 | -------------------------------------------------------------------------------- /executables/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ds main executable (pthread is used for the GUI thread) 2 | add_executable(app app.cpp) 3 | target_link_libraries(app srrg_proslam_slam_assembly_library -pthread) 4 | add_executable(srrg_proslam_app app.cpp) 5 | target_link_libraries(srrg_proslam_app srrg_proslam_slam_assembly_library -pthread) 6 | 7 | #ds ROS related - only build it if ROS is present (blindly link against the whole catkin library collection) 8 | if(${SRRG_PROSLAM_HAS_ROS}) 9 | add_executable(node node.cpp) 10 | target_link_libraries(node srrg_proslam_slam_assembly_library ${catkin_LIBRARIES}) 11 | endif() 12 | 13 | #ds g2o to kitti trajectory converter (offline bundle adjustment testing) 14 | add_executable(trajectory_converter trajectory_converter.cpp) 15 | 16 | #ds minimal stereo calibration suite 17 | add_executable(stereo_calibrator stereo_calibrator.cpp) 18 | target_link_libraries(stereo_calibrator ${OpenCV_LIBS} srrg_messages_library) 19 | 20 | #ds euroc trajectory analyzer (e.g. RMSE computation) 21 | add_executable(trajectory_analyzer trajectory_analyzer.cpp) 22 | target_link_libraries(trajectory_analyzer ${OpenCV_LIBS} srrg_core_types_library) 23 | 24 | #ds stereo triangulation and tracking test 25 | add_executable(test_stereo_frontend test_stereo_frontend.cpp) 26 | target_link_libraries(test_stereo_frontend ${OpenCV_LIBS} srrg_proslam_framepoint_generation_library) 27 | -------------------------------------------------------------------------------- /executables/README.md: -------------------------------------------------------------------------------- 1 | 2 | ProSLAM: Programmers SLAM 3 | 4 | Contributors: Dominik Schlegel, Mirco Colosi, Giorgio Grisetti
5 | 6 | **ProSLAM uses the lightning-fast, header-only [HBST library](https://gitlab.com/srrg-software/srrg_hbst) for binary descriptor similarity search (loop closing)** 7 | 8 | --- 9 | ### SLAM library applications ### 10 | **app: main application for synchronous image processing (SRRG, KITTI, TUM, ASL, MIT)** 11 | 12 | ./app 00.txt 13 | 14 | **node: main application for asynchronous image processing (ROS)** 15 | 16 | rosrun srrg_proslam node -c configuration.yaml 17 | 18 | --- 19 | ### Utilities ### 20 | 21 | **stereo_calibrator: utility for calibrating a stereo camera with an SRRG or ASL checkerboard calibration sequence (e.g. EuRoC)** 22 | 23 | ./stereo_calibrator -asl cam0 cam1 -o calibration.txt 24 | 25 | **test_stereo_frontend: utility for testing the feature-based stereo matching, triangulation and tracking (atm KITTI only)** 26 | 27 | ./test_stereo_frontend image_0/000000.png image_1/000000.png calib.txt 50 gt.txt 28 | 29 | **trajectory_analyzer: utility for loading and aligning a pair of trajectories (TUM/ASL format)** 30 | 31 | ./trajectory_analyzer -tum query_trajectory.txt -asl reference_trajectory.txt 32 | 33 | **trajectory_converter: utility for converting g2o pose graphs or TUM/ASL trajectories to KITTI format** 34 | 35 | ./trajectory_converter -g2o pose_graph.g2o 36 | 37 | --- 38 | ### It doesn't work? ### 39 | [Open an issue](https://gitlab.com/srrg-software/srrg_proslam/issues) or contact the maintainer (see package.xml) 40 | 41 | Instant troubleshooting: 42 | - 3D viewer issues (Qt) on Ubuntu 18.04? Set the enviroment variable: `QT_QPA_PLATFORMTHEME="gtk"` and try again 43 | -------------------------------------------------------------------------------- /executables/app.cpp: -------------------------------------------------------------------------------- 1 | #include "system/slam_assembly.h" 2 | 3 | int32_t main(int32_t argc_, char** argv_) { 4 | 5 | #ifdef SRRG_MERGE_DESCRIPTORS 6 | std::cerr << "main|HBST descriptor merging enabled" << std::endl; 7 | #endif 8 | 9 | //ds enable opencv optimizations 10 | cv::setUseOptimized(true); 11 | 12 | //ds allocate the complete parameter collection with default values (will be propagated through the complete SLAM system) 13 | std::cerr << "main|loading parameters" << std::endl; 14 | proslam::ParameterCollection* parameters = new proslam::ParameterCollection(proslam::LoggingLevel::Debug); 15 | 16 | //ds parse parameters from command line (optionally setting the parameter values) 17 | try { 18 | parameters->parseFromCommandLine(argc_, argv_); 19 | } catch (const std::runtime_error& exception_) { 20 | std::cerr << "main|caught exception '" << exception_.what() << "'" << std::endl; 21 | std::cerr << "main|unable to load parameters from file - terminating" << std::endl; 22 | delete parameters; 23 | return 0; 24 | } 25 | 26 | //ds print loaded configuration 27 | parameters->command_line_parameters->print(); 28 | 29 | //ds allocate SLAM system (has internal access to parameter server) 30 | proslam::SLAMAssembly slam_system(parameters); 31 | 32 | //ds worker thread 33 | std::shared_ptr slam_thread = nullptr; 34 | 35 | //ds start system 36 | try { 37 | 38 | //ds load cameras 39 | slam_system.loadCamerasFromMessageFile(); 40 | 41 | //ds if visualization is desired 42 | if (parameters->command_line_parameters->option_use_gui) { 43 | 44 | //ds enable two opencv threads 45 | cv::setNumThreads(2); 46 | 47 | //ds target maximum GUI frequency; 50 fps 48 | const proslam::real target_display_frequency = 50; 49 | const int64_t duration_gui_sleep_milliseconds = 1000/target_display_frequency; 50 | 51 | //ds allocate a qt UI server in the main scope (required) 52 | std::shared_ptr gui_server(new QApplication(argc_, argv_)); 53 | 54 | //ds initialize GUI 55 | slam_system.initializeGUI(gui_server); 56 | 57 | //ds start message playback in separate thread 58 | slam_thread = slam_system.playbackMessageFileInThread(); 59 | 60 | //ds enter GUI loop 61 | while (slam_system.isViewerOpen()) { 62 | 63 | //ds breathe (maximum GUI speed: 50 fps) 64 | std::this_thread::sleep_for(std::chrono::milliseconds(duration_gui_sleep_milliseconds)); 65 | 66 | //ds draw current state 67 | slam_system.draw(); 68 | } 69 | 70 | //ds clean up GL 71 | gui_server->closeAllWindows(); 72 | gui_server->quit(); 73 | 74 | //ds signal termination request (no effect if processing has already terminated) 75 | slam_system.requestTermination(); 76 | 77 | //ds join system thread 78 | std::cerr << "main|joining thread: system" << std::endl; 79 | slam_thread->join(); 80 | std::cerr << "main|all threads successfully joined" << std::endl; 81 | } else { 82 | 83 | //ds disable opencv multithreading 84 | cv::setNumThreads(0); 85 | 86 | //ds wait for start 87 | std::cerr << BAR << std::endl; 88 | std::cerr << "main|ready for processing - check configuration and press [ENTER] to start" << std::endl; 89 | std::cerr << BAR << std::endl; 90 | std::getchar(); 91 | 92 | //ds plain full-speed message playback in the main thread (blocking) 93 | slam_system.playbackMessageFile(); 94 | } 95 | 96 | //ds print full report 97 | slam_system.printReport(); 98 | 99 | //ds save trajectories to disk 100 | slam_system.writeTrajectoryKITTI("trajectory_kitti.txt"); 101 | slam_system.writeTrajectoryTUM("trajectory_tum.txt"); 102 | 103 | //ds save g2o graph to disk 104 | if (parameters->command_line_parameters->option_save_pose_graph) { 105 | slam_system.writePoseGraphToFile("pose_graph.g2o"); 106 | } 107 | } catch (const std::runtime_error& exception_) { 108 | std::cerr << DOUBLE_BAR << std::endl; 109 | std::cerr << "main|caught runtime exception: '" << exception_.what() << "'" << std::endl; 110 | std::cerr << DOUBLE_BAR << std::endl; 111 | 112 | //ds do not forget to join threads 113 | std::cerr << "main|joining thread: system" << std::endl; 114 | if (slam_thread) { 115 | slam_thread->join(); 116 | } 117 | std::cerr << "main|all threads successfully joined" << std::endl; 118 | } 119 | 120 | //ds clean up dynamic memory 121 | delete parameters; 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /executables/trajectory_converter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int32_t main(int32_t argc_, char** argv_) { 7 | if (argc_ < 3) { 8 | std::cerr << "usage: ./trajectory_converter -g2o | -tum " << std::endl; 9 | return 0; 10 | } 11 | 12 | //ds determine and log configuration 13 | std::string input_file = argv_[1]; 14 | std::string input_format = ""; 15 | const std::string output_file = "kitti_trajectory.txt"; 16 | const std::string output_format = "KITTI"; 17 | const std::string pose_keyword = "VERTEX_SE3:QUAT"; 18 | 19 | int32_t number_of_checked_parameters = 1; 20 | while (number_of_checked_parameters < argc_) { 21 | if (!std::strcmp(argv_[number_of_checked_parameters], "-g2o")) { 22 | input_format = "g2o"; 23 | ++number_of_checked_parameters; 24 | if (number_of_checked_parameters == argc_) {break;} 25 | input_file = argv_[number_of_checked_parameters]; 26 | } else if (!std::strcmp(argv_[number_of_checked_parameters], "-tum")) { 27 | input_format = "tum"; 28 | ++number_of_checked_parameters; 29 | if (number_of_checked_parameters == argc_) {break;} 30 | input_file = argv_[number_of_checked_parameters]; 31 | } 32 | ++number_of_checked_parameters; 33 | } 34 | 35 | std::cerr << "< converting trajectory format '" << output_format << "': '" << input_file << "'" << std::endl; 36 | std::cerr << "> to trajectory of format '" << output_format << "': '" << output_file << "'" << std::endl; 37 | 38 | if (input_format == "g2o") { 39 | std::cerr << "with pose keyword: '" << pose_keyword << "'" << std::endl; 40 | } 41 | 42 | //ds parse g2o file by hand (avoiding to link against it) 43 | std::ifstream input_stream(input_file); 44 | if (!input_stream.good() || !input_stream.is_open()) { 45 | std::cerr << "ERROR: unable to open: '" << input_file << "'" << std::endl; 46 | return 0; 47 | } 48 | 49 | //ds parsing buffers 50 | std::vector poses(0); 51 | std::string line; 52 | 53 | //ds parse the complete input file (assuming continuous, sequential indexing) 54 | while (std::getline(input_stream, line)) { 55 | 56 | //ds for g2o 57 | if (input_format == "g2o") { 58 | 59 | //ds check if the current line contains pose information 60 | if (line.find(pose_keyword) != std::string::npos) { 61 | 62 | //ds get line to a string stream object 63 | std::istringstream stringstream(line); 64 | 65 | //ds possible values 66 | std::string entry_name; 67 | uint64_t entry_identifier = 0; 68 | double translation_x = 0; 69 | double translation_y = 0; 70 | double translation_z = 0; 71 | double quaternion_w = 0; 72 | double quaternion_x = 0; 73 | double quaternion_y = 0; 74 | double quaternion_z = 0; 75 | 76 | //ds parse the full line and check for failure 77 | if (!(stringstream >> entry_name >> entry_identifier >> translation_x >> translation_y >> translation_z 78 | >> quaternion_x >> quaternion_y >> quaternion_z >> quaternion_w)) { 79 | std::cerr << "ERROR: unable to parse pose lines with keyword: '" << pose_keyword << "'" << std::endl; 80 | input_stream.close(); 81 | return 0; 82 | } 83 | 84 | //ds set pose value 85 | Eigen::Isometry3d parsed_pose(Eigen::Isometry3d::Identity()); 86 | parsed_pose.translation() = Eigen::Vector3d(translation_x, translation_y, translation_z); 87 | parsed_pose.linear() = Eigen::Quaterniond(quaternion_w, quaternion_x, quaternion_y, quaternion_z).toRotationMatrix(); 88 | poses.push_back(parsed_pose); 89 | } 90 | } else if (input_format == "tum") { 91 | 92 | //ds get line to a string stream object 93 | std::istringstream stringstream(line); 94 | 95 | //ds possible values 96 | double time_stamp = 0; 97 | double translation_x = 0; 98 | double translation_y = 0; 99 | double translation_z = 0; 100 | double quaternion_w = 0; 101 | double quaternion_x = 0; 102 | double quaternion_y = 0; 103 | double quaternion_z = 0; 104 | 105 | //ds parse the full line and check for failure 106 | if (!(stringstream >> time_stamp >> translation_x >> translation_y >> translation_z 107 | >> quaternion_x >> quaternion_y >> quaternion_z >> quaternion_w)) { 108 | std::cerr << "ERROR: unable to parse pose lines" << std::endl; 109 | input_stream.close(); 110 | return 0; 111 | } 112 | 113 | //ds set pose value 114 | Eigen::Isometry3d parsed_pose(Eigen::Isometry3d::Identity()); 115 | parsed_pose.translation() = Eigen::Vector3d(translation_x, translation_y, translation_z); 116 | parsed_pose.linear() = Eigen::Quaterniond(quaternion_w, quaternion_x, quaternion_y, quaternion_z).toRotationMatrix(); 117 | poses.push_back(parsed_pose); 118 | } 119 | } 120 | input_stream.close(); 121 | std::cerr << "parsed poses: " << poses.size() << std::endl; 122 | 123 | //ds open output file (assuming continuous, sequential indexing) 124 | std::ofstream output_stream(output_file, std::ifstream::out); 125 | for (const Eigen::Isometry3d& pose: poses) { 126 | 127 | //ds dump transform according to KITTI format 128 | for (uint8_t u = 0; u < 3; ++u) { 129 | for (uint8_t v = 0; v < 4; ++v) { 130 | output_stream << pose(u,v) << " "; 131 | } 132 | } 133 | output_stream << "\n"; 134 | } 135 | output_stream.close(); 136 | std::cerr << "to trajectory of format '" << output_format << "' to: '" << output_file << "'" << std::endl; 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg_proslam 4 | 1.0.0 5 | The ProSLAM package 6 | Dominik Schlegel 7 | BSD 8 | catkin 9 | srrg_cmake_modules 10 | srrg_core 11 | srrg_hbst 12 | srrg_core_viewers 13 | srrg_gl_helpers 14 | g2o_catkin 15 | g2o_slim_edu 16 | roscpp 17 | sensor_msg 18 | cv_bridge 19 | nav_msgs 20 | message_filters 21 | srrg_core 22 | srrg_hbst 23 | srrg_core_viewers 24 | srrg_gl_helpers 25 | g2o_catkin 26 | g2o_slim_edu 27 | roscpp 28 | sensor_msgs 29 | cv_bridge 30 | nav_msgs 31 | message_filters 32 | 33 | -------------------------------------------------------------------------------- /pull_srrg_packages.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #ds script is assumed to be called from within proslam project root (brutal) 3 | DIR_ORIGINAL=`pwd` 4 | #ds move one level up (must be src now, brutal) 5 | cd .. 6 | DIR_CATKIN_SOURCE=`pwd` 7 | echo "- updating SRRG packages in: ${DIR_CATKIN_SOURCE} (this should be your catkin workspace source directory)" 8 | cd ${DIR_CATKIN_SOURCE} 9 | 10 | #ds start loading packages - first check if they exist 11 | if [ -d "${DIR_CATKIN_SOURCE}/srrg_cmake_modules" ]; then 12 | echo "- srrg_cmake_modules already installed - updating code" 13 | cd "${DIR_CATKIN_SOURCE}/srrg_cmake_modules" 14 | git pull 15 | else 16 | cd "${DIR_CATKIN_SOURCE}" 17 | echo "- installing srrg_cmake_modules:" 18 | git clone https://gitlab.com/srrg-software/srrg_cmake_modules 19 | fi 20 | if [ -d "${DIR_CATKIN_SOURCE}/srrg_core" ]; then 21 | echo "- srrg_core already installed - updating code" 22 | cd "${DIR_CATKIN_SOURCE}/srrg_core" 23 | git pull 24 | else 25 | cd "${DIR_CATKIN_SOURCE}" 26 | echo "- installing srrg_core:" 27 | git clone https://gitlab.com/srrg-software/srrg_core 28 | fi 29 | if [ -d "${DIR_CATKIN_SOURCE}/srrg_gl_helpers" ]; then 30 | echo "- srrg_gl_helpers already installed - updating code" 31 | cd "${DIR_CATKIN_SOURCE}/srrg_gl_helpers" 32 | git pull 33 | else 34 | cd "${DIR_CATKIN_SOURCE}" 35 | echo "- installing srrg_gl_helpers:" 36 | git clone https://gitlab.com/srrg-software/srrg_gl_helpers 37 | fi 38 | if [ -d "${DIR_CATKIN_SOURCE}/srrg_core_viewers" ]; then 39 | echo "- srrg_core_viewers already installed - updating code" 40 | cd "${DIR_CATKIN_SOURCE}/srrg_core_viewers" 41 | git pull 42 | else 43 | cd "${DIR_CATKIN_SOURCE}" 44 | echo "- installing srrg_core_viewers:" 45 | git clone https://gitlab.com/srrg-software/srrg_core_viewers 46 | fi 47 | if [ -d "${DIR_CATKIN_SOURCE}/srrg_hbst" ]; then 48 | echo "- srrg_hbst already installed - updating code" 49 | cd "${DIR_CATKIN_SOURCE}/srrg_hbst" 50 | git pull 51 | else 52 | cd "${DIR_CATKIN_SOURCE}" 53 | echo "- installing srrg_hbst:" 54 | git clone https://gitlab.com/srrg-software/srrg_hbst 55 | fi 56 | 57 | #ds notify and return to original folder 58 | echo "- done" 59 | cd ${DIR_ORIGINAL} 60 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(aligners) 2 | add_subdirectory(framepoint_generation) 3 | add_subdirectory(map_optimization) 4 | add_subdirectory(position_tracking) 5 | add_subdirectory(relocalization) 6 | add_subdirectory(types) 7 | add_subdirectory(visualization) 8 | add_subdirectory(system) 9 | -------------------------------------------------------------------------------- /src/aligners/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_aligners_library 2 | stereouv_aligner.cpp 3 | uvd_aligner.cpp 4 | xyz_aligner.cpp 5 | ) 6 | 7 | target_link_libraries(srrg_proslam_aligners_library 8 | srrg_proslam_types_library 9 | ) 10 | -------------------------------------------------------------------------------- /src/aligners/base_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/definitions.h" 3 | 4 | namespace proslam { 5 | 6 | //ds base aligner class 7 | class BaseAligner { 8 | 9 | //ds object handling 10 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | //! @brief default constructor 13 | //! @param[in] parameters_ target parameters handle 14 | BaseAligner(AlignerParameters* parameters_): _parameters(parameters_) {}; 15 | 16 | //! @brief configuration method, called once all construction parameters are set 17 | virtual void configure() {} 18 | 19 | //! @brief default destructor 20 | virtual ~BaseAligner() {}; 21 | 22 | //ds functionality 23 | public: 24 | 25 | //ds linearize the system: to be called inside oneRound 26 | virtual void linearize(const bool& ignore_outliers_) = 0; 27 | 28 | //ds solve alignment problem for one round: calling linearize 29 | virtual void oneRound(const bool& ignore_outliers_) = 0; 30 | 31 | //ds solve alignment problem until convergence is reached 32 | virtual void converge() = 0; 33 | 34 | //ds getters/setters 35 | public: 36 | 37 | inline const std::vector& errors() const {return _errors;} 38 | inline const std::vector& inliers() const {return _inliers;} 39 | inline const uint64_t numberOfInliers() const {return _number_of_inliers;} 40 | inline const uint64_t numberOfOutliers() const {return _number_of_outliers;} 41 | inline const uint64_t numberOfCorrespondences() const {return _number_of_inliers+_number_of_outliers;} 42 | inline const real totalError() const {return _total_error;} 43 | inline const bool hasSystemConverged() const {return _has_system_converged;} 44 | inline AlignerParameters* parameters() {return _parameters;} 45 | 46 | //ds aligner specific 47 | protected: 48 | 49 | //ds control 50 | std::vector _errors; 51 | std::vector _inliers; 52 | uint64_t _number_of_inliers = 0; 53 | uint64_t _number_of_outliers = 0; 54 | 55 | //ds linearization 56 | bool _has_system_converged = false; 57 | real _total_error = 0; 58 | 59 | //! @brief configurable parameters 60 | AlignerParameters* _parameters = 0; 61 | 62 | }; 63 | 64 | //ds class that holds all generic (running) variables used in an aligner - to be used in conjunction with an aligner 65 | template 66 | class AlignerWorkspace { 67 | 68 | //ds object handling 69 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | //ds default workspace setup 72 | AlignerWorkspace() {} 73 | 74 | //ds default workspace tear-down 75 | ~AlignerWorkspace() {} 76 | 77 | //ds exported types 78 | public: 79 | 80 | typedef Eigen::Matrix StateMatrix; 81 | typedef Eigen::Matrix StateVector; 82 | typedef Eigen::Matrix DimensionMatrix; 83 | typedef Eigen::Matrix JacobianMatrix; 84 | 85 | //ds workspace variables 86 | protected: 87 | 88 | StateMatrix _H = StateMatrix::Zero(); 89 | StateVector _b = StateVector::Zero(); 90 | DimensionMatrix _omega = DimensionMatrix::Identity(); 91 | JacobianMatrix _jacobian = JacobianMatrix::Zero(); 92 | StateMatrix _information_matrix = StateMatrix::Identity(); 93 | }; 94 | } 95 | -------------------------------------------------------------------------------- /src/aligners/base_frame_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/frame.h" 3 | #include "base_aligner.h" 4 | 5 | namespace proslam { 6 | 7 | //ds implements an interface for frame-to-frame aligner to be used inside the tracker 8 | class BaseFrameAligner: public BaseAligner { 9 | 10 | //ds object handling 11 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | BaseFrameAligner(AlignerParameters* parameters_): BaseAligner(parameters_) {} 14 | virtual ~BaseFrameAligner() {}; 15 | 16 | //ds functionality 17 | public: 18 | 19 | //ds pure virtual 20 | virtual void initialize(const Frame* frame_previous_, 21 | const Frame* frame_current_, 22 | const TransformMatrix3D& previous_to_current_) = 0; 23 | 24 | //ds setters/getters 25 | public: 26 | 27 | void setMaximumDepthNearMeters(const real& maximum_depth_near_meters_) {_maximum_depth_near_meters = maximum_depth_near_meters_;} 28 | void setMaximumDepthFarMeters(const real& maximum_depth_far_meters_) {_maximum_depth_far_meters = maximum_depth_far_meters_;} 29 | inline const TransformMatrix3D& previousToCurrent() const {return _previous_to_current;} 30 | 31 | //ds dynamic weighting 32 | void setEnableWeightsTranslation(const bool enable_weights_translation_) {_enable_weights_translation = enable_weights_translation_;} 33 | 34 | //ds attributes 35 | protected: 36 | 37 | //ds alignment context 38 | const Frame* _frame_current = 0; 39 | const Frame* _frame_previous = 0; 40 | 41 | //ds objective 42 | TransformMatrix3D _previous_to_current = TransformMatrix3D::Identity(); 43 | 44 | real _maximum_depth_near_meters = 0; 45 | real _maximum_depth_far_meters = 0; 46 | 47 | Count _number_of_rows_image = 0; 48 | Count _number_of_cols_image = 0; 49 | 50 | bool _enable_weights_translation = true; 51 | }; 52 | } 53 | -------------------------------------------------------------------------------- /src/aligners/base_local_map_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/local_map.h" 3 | #include "relocalization/closure.h" 4 | #include "base_aligner.h" 5 | 6 | namespace proslam { 7 | 8 | //ds class that implements an aligner interface between two local maps 9 | class BaseLocalMapAligner: public BaseAligner { 10 | 11 | //ds object handling 12 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | BaseLocalMapAligner(AlignerParameters* parameters_): BaseAligner(parameters_) {} 15 | virtual ~BaseLocalMapAligner() {}; 16 | 17 | //ds functionality 18 | public: 19 | 20 | //ds pure virtual 21 | virtual void initialize(Closure* context_, const TransformMatrix3D& current_to_reference_ = TransformMatrix3D::Identity()) = 0; 22 | 23 | //ds setters/getters 24 | public: 25 | 26 | inline const TransformMatrix3D& currentToReference() const {return _current_to_reference;} 27 | 28 | protected: 29 | 30 | //ds context 31 | Closure* _context = 0; 32 | 33 | //ds objective 34 | TransformMatrix3D _current_to_reference = TransformMatrix3D::Identity(); 35 | }; 36 | } 37 | -------------------------------------------------------------------------------- /src/aligners/stereouv_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_frame_aligner.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class specifies an aligner for pose optimization by minimizing the reprojection errors in the image plane (used to determine the robots odometry) 7 | class StereoUVAligner: public BaseFrameAligner, public AlignerWorkspace<6, 4> { 8 | 9 | //ds object handling 10 | PROSLAM_MAKE_PROCESSING_SUBCLASS(StereoUVAligner, AlignerParameters) 11 | 12 | //ds functionality 13 | public: 14 | 15 | //ds initialize aligner with minimal entity 16 | virtual void initialize(const Frame* frame_previous_, 17 | const Frame* frame_current_, 18 | const TransformMatrix3D& previous_to_current_); 19 | 20 | //ds linearize the system: to be called inside oneRound 21 | virtual void linearize(const bool& ignore_outliers_); 22 | 23 | //ds solve alignment problem for one round: to be called inside converge 24 | virtual void oneRound(const bool& ignore_outliers_); 25 | 26 | //ds solve alignment problem until convergence is reached 27 | virtual void converge(); 28 | 29 | //ds set maximum reliable depth for registration 30 | void setMaximumReliableDepthMeters(const double& maximum_reliable_depth_meters_) {_maximum_reliable_depth_meters = maximum_reliable_depth_meters_;} 31 | 32 | //ds aligner specific 33 | protected: 34 | 35 | //ds buffers 36 | CameraMatrix _camera_calibration_matrix = CameraMatrix::Zero(); 37 | Vector3 _offset_camera_right = Vector3::Zero(); 38 | real _minimum_depth = 1.0; 39 | 40 | //ds module paramaters 41 | real _maximum_reliable_depth_meters = 15; 42 | 43 | Count _number_of_measurements = 0; 44 | std::vector > _information_vector; 45 | std::vector > _moving; 46 | std::vector > _fixed; 47 | std::vector _weights_translation; 48 | }; 49 | } 50 | -------------------------------------------------------------------------------- /src/aligners/uvd_aligner.cpp: -------------------------------------------------------------------------------- 1 | #include "uvd_aligner.h" 2 | 3 | #include "types/landmark.h" 4 | 5 | namespace proslam { 6 | using namespace srrg_core; 7 | 8 | UVDAligner::UVDAligner(AlignerParameters* parameters_): BaseFrameAligner(parameters_) { 9 | //ds nothing to do 10 | } 11 | 12 | UVDAligner::~UVDAligner() { 13 | //ds nothing to do 14 | } 15 | 16 | //ds initialize aligner with minimal entity 17 | void UVDAligner::initialize(const Frame* frame_previous_, 18 | const Frame* frame_current_, 19 | const TransformMatrix3D& previous_to_current_) { 20 | _frame_current = frame_current_; 21 | _errors.resize(_frame_current->points().size()); 22 | _inliers.resize(_frame_current->points().size()); 23 | // TODO _robot_to_world = previous_to_current_; 24 | // _world_to_robot = _robot_to_world.inverse(); 25 | 26 | //ds wrappers for optimization 27 | // _camera_to_world = _robot_to_world*_frame_current->cameraLeft()->cameraToRobot(); 28 | // _world_to_camera = _camera_to_world.inverse(); 29 | _camera_matrix = _frame_current->cameraLeft()->cameraMatrix(); 30 | _number_of_rows_image = _frame_current->cameraLeft()->numberOfImageRows(); 31 | _number_of_cols_image = _frame_current->cameraLeft()->numberOfImageCols(); 32 | } 33 | 34 | //ds linearize the system: to be called inside oneRound 35 | void UVDAligner::linearize(const bool& ignore_outliers_) { 36 | 37 | //ds initialize setup 38 | _H.setZero(); 39 | _b.setZero(); 40 | _number_of_inliers = 0; 41 | _number_of_outliers = 0; 42 | _total_error = 0; 43 | 44 | //ds loop over all points (assumed to have previous points) 45 | for (Index index_point = 0; index_point < _frame_current->points().size(); index_point++) { 46 | _errors[index_point] = -1; 47 | _inliers[index_point] = false; 48 | _omega.setIdentity(); 49 | _omega(2,2)*=10; 50 | 51 | //ds buffer framepoint 52 | FramePoint* frame_point = _frame_current->points()[index_point]; 53 | assert(_frame_current->cameraLeft()->isInFieldOfView(frame_point->imageCoordinatesLeft())); 54 | assert(frame_point->previous()); 55 | 56 | //ds buffer landmark 57 | const Landmark* landmark = frame_point->landmark(); 58 | 59 | //ds compute the point in the camera frame - prefering a landmark estimate if available 60 | PointCoordinates predicted_point_in_camera = PointCoordinates::Zero(); 61 | if (landmark) { 62 | // predicted_point_in_camera = _world_to_camera*landmark->coordinates(); 63 | _omega *= 1.5; 64 | } else { 65 | // predicted_point_in_camera = _world_to_camera*frame_point->previous()->worldCoordinates(); 66 | } 67 | const real& depth_meters = predicted_point_in_camera.z(); 68 | if (depth_meters <= 0 || depth_meters > _maximum_depth_far_meters) { 69 | continue; 70 | } 71 | 72 | //ds retrieve homogeneous projections 73 | const PointCoordinates predicted_uvd_in_camera = _camera_matrix*predicted_point_in_camera; 74 | 75 | //ds compute the image coordinates 76 | PointCoordinates predicted_point_in_image = predicted_uvd_in_camera/depth_meters; 77 | //restore the depth in the third component 78 | predicted_point_in_image.z()=depth_meters; 79 | 80 | //ds if the point is outside the image, skip 81 | if (predicted_point_in_image.x() < 0 || predicted_point_in_image.x() > _number_of_cols_image|| 82 | predicted_point_in_image.y() < 0 || predicted_point_in_image.y() > _number_of_rows_image) { 83 | continue; 84 | } 85 | 86 | assert(_frame_current->cameraLeft()->isInFieldOfView(predicted_point_in_image)); 87 | 88 | //ds precompute 89 | const real inverse_predicted_d = 1/depth_meters; 90 | const real inverse_predicted_d_squared = inverse_predicted_d*inverse_predicted_d; 91 | 92 | //ds compute error 93 | const Vector3 error(predicted_point_in_image.x()-frame_point->imageCoordinatesLeft().x(), 94 | predicted_point_in_image.y()-frame_point->imageCoordinatesLeft().y(), 95 | predicted_point_in_image.z()-frame_point->cameraCoordinatesLeft().z()); 96 | 97 | //ds compute squared error 98 | const real chi = error.transpose()*error; 99 | 100 | //ds update error stats 101 | _errors[index_point] = chi; 102 | 103 | //ds if outlier 104 | if (chi > _parameters->maximum_error_kernel) { 105 | ++_number_of_outliers; 106 | if (ignore_outliers_) { 107 | continue; 108 | } 109 | 110 | //ds include kernel in omega 111 | _omega *= _parameters->maximum_error_kernel/chi; 112 | } else { 113 | _inliers[index_point] = true; 114 | ++_number_of_inliers; 115 | } 116 | 117 | //ds update total error 118 | _total_error += _errors[index_point]; 119 | 120 | //ds compute the jacobian of the transformation 121 | Matrix3_6 jacobian_transform; 122 | jacobian_transform.setZero(); 123 | 124 | //ds if the point is near enough - we consider translation 125 | if (depth_meters < _maximum_depth_near_meters) { 126 | jacobian_transform.block<3,3>(0,0).setIdentity(); 127 | } 128 | 129 | //ds always consider rotation 130 | jacobian_transform.block<3,3>(0,3) = -2*skew(predicted_point_in_camera); 131 | 132 | //ds jacobian parts of the homogeneous division 133 | Matrix3 jacobian_projection; 134 | jacobian_projection << 135 | inverse_predicted_d, 0, -predicted_uvd_in_camera.x()*inverse_predicted_d_squared, 136 | 0, inverse_predicted_d, -predicted_uvd_in_camera.y()*inverse_predicted_d_squared, 137 | 0, 0, 1; 138 | 139 | //ds assemble final jacobian 140 | _jacobian = jacobian_projection*_camera_matrix*jacobian_transform; 141 | 142 | //ds precompute transposed 143 | const Matrix6_3 jacobian_transposed(_jacobian.transpose()); 144 | 145 | if (depth_meters < _maximum_depth_near_meters) { 146 | _omega *= (_maximum_depth_near_meters-depth_meters)/_maximum_depth_near_meters; 147 | } else { 148 | _omega *= (_maximum_depth_far_meters-depth_meters)/_maximum_depth_far_meters; 149 | } 150 | 151 | //ds update H and b 152 | _H += jacobian_transposed*_omega*_jacobian; 153 | _b += jacobian_transposed*_omega*error; 154 | } 155 | } 156 | 157 | //ds solve alignment problem for one round 158 | void UVDAligner::oneRound(const bool& ignore_outliers_) { 159 | throw std::runtime_error("UVDAligner::oneRound|TODO: sanitize"); 160 | } 161 | 162 | //ds solve alignment problem until convergence is reached 163 | void UVDAligner::converge() { 164 | 165 | //ds previous error to check for convergence 166 | real total_error_previous = 0; 167 | 168 | //ds start LS 169 | for (Count iteration = 0; iteration < _parameters->maximum_number_of_iterations; ++iteration) { 170 | oneRound(false); 171 | 172 | //ds check if converged (no descent required) 173 | if (_parameters->error_delta_for_convergence > std::fabs(total_error_previous-_total_error)) { 174 | 175 | //ds trigger inlier only runs 176 | oneRound(true); 177 | oneRound(true); 178 | oneRound(true); 179 | 180 | //ds compute information matrix 181 | _information_matrix = _H; 182 | 183 | //ds system converged 184 | _has_system_converged = true; 185 | break; 186 | } else { 187 | total_error_previous = _total_error; 188 | } 189 | 190 | //ds check last iteration 191 | if(iteration == _parameters->maximum_number_of_iterations-1) { 192 | _has_system_converged = false; 193 | LOG_WARNING(std::cerr << "UVDAligner::converge|system did not converge - total error: " << _total_error 194 | << " average error: " << _total_error/(_number_of_inliers+_number_of_outliers) 195 | << " inliers: " << _number_of_inliers << " outliers: " << _number_of_outliers << std::endl) 196 | } 197 | } 198 | 199 | // //ds update wrapped structures 200 | // _camera_to_world = _world_to_camera.inverse(); 201 | // _robot_to_world = _camera_to_world*_frame_current->cameraLeft()->robotToCamera(); 202 | // _world_to_robot = _robot_to_world.inverse(); 203 | } 204 | } 205 | -------------------------------------------------------------------------------- /src/aligners/uvd_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_frame_aligner.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class specifies an aligner for pose optimization by minimizing the reprojection errors in the image plane (used to determine the robots odometry) 7 | class UVDAligner: public BaseFrameAligner, public AlignerWorkspace<6,3> { 8 | 9 | //ds object handling 10 | PROSLAM_MAKE_PROCESSING_SUBCLASS(UVDAligner, AlignerParameters) 11 | 12 | //ds functionality 13 | public: 14 | 15 | //ds initialize aligner with minimal entity 16 | virtual void initialize(const Frame* frame_previous_, 17 | const Frame* frame_current_, 18 | const TransformMatrix3D& previous_to_current_); 19 | 20 | //ds linearize the system: to be called inside oneRound 21 | virtual void linearize(const bool& ignore_outliers_); 22 | 23 | //ds solve alignment problem for one round: to be called inside converge 24 | virtual void oneRound(const bool& ignore_outliers_); 25 | 26 | //ds solve alignment problem until convergence is reached 27 | virtual void converge(); 28 | 29 | //ds aligner specific 30 | protected: 31 | 32 | //ds buffers 33 | CameraMatrix _camera_matrix = CameraMatrix::Zero(); 34 | }; 35 | } 36 | -------------------------------------------------------------------------------- /src/aligners/xyz_aligner.cpp: -------------------------------------------------------------------------------- 1 | #include "xyz_aligner.h" 2 | 3 | namespace proslam { 4 | 5 | XYZAligner::XYZAligner(AlignerParameters* parameters_): BaseLocalMapAligner(parameters_) { 6 | //ds nothing to do 7 | } 8 | 9 | XYZAligner::~XYZAligner() { 10 | //ds nothing to do 11 | } 12 | 13 | void XYZAligner::initialize(Closure* context_, const TransformMatrix3D& current_to_reference_) { 14 | 15 | //ds initialize base components 16 | _context = context_; 17 | _current_to_reference = current_to_reference_; 18 | _parameters->damping = 0; 19 | _number_of_measurements = _context->correspondences.size(); 20 | _errors.resize(_number_of_measurements); 21 | _inliers.resize(_number_of_measurements); 22 | 23 | //ds construct point cloud registration problem - compute landmark coordinates in local maps 24 | _information_vector.resize(_number_of_measurements); 25 | _moving.resize(_number_of_measurements); 26 | _fixed.resize(_number_of_measurements); 27 | const TransformMatrix3D& world_to_reference_local_map(context_->local_map_reference->worldToLocalMap()); 28 | const TransformMatrix3D& world_to_query_local_map(context_->local_map_query->worldToLocalMap()); 29 | for (Index u = 0; u < _number_of_measurements; ++u) { 30 | const Closure::Correspondence* correspondence = _context->correspondences[u]; 31 | 32 | //ds point coordinates to register 33 | _fixed[u] = world_to_reference_local_map*correspondence->reference->coordinates(); 34 | _moving[u] = world_to_query_local_map*correspondence->query->coordinates(); 35 | 36 | //ds set information matrix 37 | _information_vector[u].setIdentity(); 38 | _information_vector[u] *= correspondence->matching_ratio; 39 | } 40 | } 41 | 42 | void XYZAligner::linearize(const bool& ignore_outliers_) { 43 | 44 | //ds initialize setup 45 | _H.setZero(); 46 | _b.setZero(); 47 | _number_of_inliers = 0; 48 | _number_of_outliers = 0; 49 | _total_error = 0; 50 | 51 | //ds for all the points 52 | for (Index u = 0; u < _number_of_measurements; ++u) { 53 | _omega = _information_vector[u]; 54 | 55 | //ds compute error based on items: local map merging 56 | const PointCoordinates sampled_point_in_reference = _current_to_reference*_moving[u]; 57 | const Vector3 error = sampled_point_in_reference-_fixed[u]; 58 | 59 | //ds update chi 60 | const real error_squared = error.transpose()*_omega*error; 61 | 62 | //ds check if outlier 63 | if (error_squared > _parameters->maximum_error_kernel) { 64 | _inliers[u] = false; 65 | ++_number_of_outliers; 66 | if (ignore_outliers_) { 67 | continue; 68 | } 69 | 70 | //ds proportionally reduce information value of the measurement 71 | _omega *= _parameters->maximum_error_kernel/error_squared; 72 | } else { 73 | _inliers[u] = true; 74 | ++_number_of_inliers; 75 | } 76 | _total_error += error_squared; 77 | 78 | //ds get the jacobian of the transform part = [I -2*skew(T*modelPoint)] 79 | _jacobian.block<3,3>(0,0).setIdentity(); 80 | _jacobian.block<3,3>(0,3) = -2*srrg_core::skew(sampled_point_in_reference); 81 | 82 | //ds precompute transposed 83 | const Matrix6_3 jacobian_transposed(_jacobian.transpose( )); 84 | 85 | //ds accumulate 86 | _H += jacobian_transposed*_omega*_jacobian; 87 | _b += jacobian_transposed*_omega*error; 88 | } 89 | } 90 | 91 | void XYZAligner::oneRound(const bool& ignore_outliers_) { 92 | 93 | //ds linearize system 94 | linearize(ignore_outliers_); 95 | 96 | //ds solve the system and update the estimate 97 | _current_to_reference = srrg_core::v2t(static_cast(_H.ldlt().solve(-_b)))*_current_to_reference; 98 | 99 | //ds enforce rotation symmetry 100 | const Matrix3 rotation = _current_to_reference.linear(); 101 | Matrix3 rotation_squared = rotation.transpose( )*rotation; 102 | rotation_squared.diagonal().array() -= 1; 103 | _current_to_reference.linear() -= 0.5*rotation*rotation_squared; 104 | } 105 | 106 | void XYZAligner::converge() { 107 | 108 | //ds previous error to check for convergence 109 | real total_error_previous = 0.0; 110 | 111 | //ds start LS 112 | for (Count iteration = 0; iteration < _parameters->maximum_number_of_iterations; ++iteration) { 113 | oneRound(false); 114 | 115 | //ds check if converged (no descent required) 116 | if (_parameters->error_delta_for_convergence > std::fabs(total_error_previous-_total_error)) { 117 | 118 | //ds trigger inlier only runs 119 | oneRound(true); 120 | oneRound(true); 121 | oneRound(true); 122 | 123 | //ds system converged 124 | _has_system_converged = true; 125 | 126 | //ds compute inliers ratio 127 | const real inlier_ratio = static_cast(_number_of_inliers)/_context->correspondences.size(); 128 | 129 | //ds set out values 130 | _context->query_to_reference = _current_to_reference; 131 | _context->icp_inlier_ratio = inlier_ratio; 132 | _context->icp_number_of_inliers = _number_of_inliers; 133 | _context->icp_number_of_iterations = iteration; 134 | 135 | //ds if the solution is acceptable 136 | if (_number_of_inliers > _parameters->minimum_number_of_inliers && inlier_ratio > _parameters->minimum_inlier_ratio) { 137 | LOG_INFO(std::printf("XYZAligner::converge|registered local maps [%06lu:{%06lu-%06lu}] > [%06lu:{%06lu-%06lu}] " 138 | "(correspondences: %3lu, iterations: %2lu, inlier ratio: %5.3f, inliers: %2lu)\n", 139 | _context->local_map_query->identifier(), 140 | _context->local_map_query->frames().front()->identifier(), _context->local_map_query->frames().back()->identifier(), 141 | _context->local_map_reference->identifier(), 142 | _context->local_map_reference->frames().front()->identifier(), _context->local_map_reference->frames().back()->identifier(), 143 | _context->correspondences.size(), iteration, inlier_ratio, _number_of_inliers)) 144 | 145 | //ds enable closure 146 | _context->is_valid = true; 147 | 148 | //ds set inlier status 149 | for (Index u = 0; u < _number_of_measurements; ++u) { 150 | _context->correspondences[u]->is_inlier = _inliers[u]; 151 | } 152 | 153 | break; 154 | } else { 155 | LOG_DEBUG(std::printf("XYZAligner::converge|dropped registration for local maps [%06lu:{%06lu-%06lu}] > [%06lu:{%06lu-%06lu}] " 156 | "(correspondences: %3lu, iterations: %2lu, inlier ratio: %5.3f, inliers: %2lu)\n", 157 | _context->local_map_query->identifier(), 158 | _context->local_map_query->frames().front()->identifier(), _context->local_map_query->frames().back()->identifier(), 159 | _context->local_map_reference->identifier(), 160 | _context->local_map_reference->frames().front()->identifier(), _context->local_map_reference->frames().back()->identifier(), 161 | _context->correspondences.size(), iteration, inlier_ratio, _number_of_inliers)) 162 | _context->is_valid = false; 163 | break; 164 | } 165 | } else { 166 | total_error_previous = _total_error; 167 | } 168 | 169 | //ds check last iteration 170 | if(iteration == _parameters->maximum_number_of_iterations-1) { 171 | _context->is_valid = false; 172 | _has_system_converged = false; 173 | LOG_DEBUG(std::cerr << "XYZAligner::converge|system did not converge - inlier ratio: " << static_cast(_number_of_inliers)/_context->correspondences.size() 174 | << " [" << _context->local_map_query->identifier() << "][" << _context->local_map_reference->identifier() << "]" << std::endl) 175 | } 176 | } 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /src/aligners/xyz_aligner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_local_map_aligner.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class specifies an aligner for camera centric point clouds (used to compute the spatial relation between local maps for a loop closure) 7 | class XYZAligner: public BaseLocalMapAligner, public AlignerWorkspace<6,3> { 8 | 9 | //ds object handling 10 | PROSLAM_MAKE_PROCESSING_SUBCLASS(XYZAligner, AlignerParameters) 11 | 12 | //ds functionality 13 | public: 14 | 15 | //ds initialize aligner with minimal entity 16 | virtual void initialize(Closure* context_, const TransformMatrix3D& current_to_reference_ = TransformMatrix3D::Identity()); 17 | 18 | //ds linearize the system: to be called inside oneRound 19 | virtual void linearize(const bool& ignore_outliers_); 20 | 21 | //ds solve alignment problem for one round: to be called inside converge 22 | virtual void oneRound(const bool& ignore_outliers_); 23 | 24 | //ds solve alignment problem until convergence is reached 25 | virtual void converge(); 26 | 27 | //ds attributes 28 | protected: 29 | 30 | //ds solver setup (TODO port solver) 31 | Count _number_of_measurements = 0; 32 | std::vector > _information_vector; 33 | std::vector _moving; 34 | std::vector _fixed; 35 | 36 | }; 37 | 38 | typedef std::shared_ptr XYZAlignerPtr; 39 | 40 | } 41 | -------------------------------------------------------------------------------- /src/framepoint_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_framepoint_generation_library 2 | intensity_feature_matcher.cpp 3 | base_framepoint_generator.cpp 4 | stereo_framepoint_generator.cpp 5 | depth_framepoint_generator.cpp 6 | ) 7 | 8 | target_link_libraries(srrg_proslam_framepoint_generation_library 9 | srrg_proslam_types_library 10 | ) 11 | -------------------------------------------------------------------------------- /src/framepoint_generation/base_framepoint_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/frame.h" 3 | #include "intensity_feature_matcher.h" 4 | 5 | 6 | 7 | namespace proslam { 8 | 9 | //! @class this class computes potential framepoints in a stereo image pair by triangulation 10 | class BaseFramePointGenerator { 11 | 12 | //ds object handling 13 | PROSLAM_MAKE_PROCESSING_CLASS(BaseFramePointGenerator) 14 | 15 | //ds functionality 16 | public: 17 | 18 | //ds initializes the framepoint generator (e.g. detects keypoints and computes descriptors) 19 | virtual void initialize(Frame* frame_, const bool& extract_features_ = true) = 0; 20 | 21 | //ds computes framepoints stored in a image-like matrix (_framepoints_in_image) 22 | virtual void compute(Frame* frame_) = 0; 23 | 24 | //ds detects keypoints and stores them in a vector (called within compute) 25 | void detectKeypoints(const cv::Mat& intensity_image_, std::vector& keypoints_); 26 | 27 | //ds extracts the defined descriptors for the given keypoints (called within compute) 28 | void computeDescriptors(const cv::Mat& intensity_image_, std::vector& keypoints_, cv::Mat& descriptors_); 29 | 30 | //@ brief computes tracks between current and previous image points based on appearance 31 | //! @param[out] previous_points_without_tracks_ lost points 32 | virtual void track(Frame* frame_, 33 | Frame* frame_previous_, 34 | const TransformMatrix3D& camera_left_previous_in_current_, 35 | FramePointPointerVector& previous_framepoints_without_tracks_, 36 | const bool track_by_appearance_ = true); 37 | 38 | //ds adjust detector thresholds (for all image streams) 39 | void adjustDetectorThresholds(); 40 | 41 | //ds getters/setters 42 | public: 43 | 44 | //ds enable external access to descriptor extractor 45 | cv::Ptr descriptorExtractor() const {return _descriptor_extractor;} 46 | 47 | //ds other properties 48 | void setCameraLeft(const Camera* camera_left_) {_camera_left = camera_left_;} 49 | const int32_t& numberOfRowsImage() const {return _number_of_rows_image;} 50 | const int32_t& numberOfColsImage() const {return _number_of_cols_image;} 51 | const Count& targetNumberOfKeypoints() const {return _target_number_of_keypoints;} 52 | void setProjectionTrackingDistancePixels(const int32_t& projection_tracking_distance_pixels_) {_projection_tracking_distance_pixels = projection_tracking_distance_pixels_;} 53 | 54 | const int32_t matchingDistanceTrackingThreshold() const {return _parameters->matching_distance_tracking_threshold;} 55 | const Count& numberOfDetectedKeypoints() const {return _number_of_detected_keypoints;} 56 | const Count& numberOfTrackedLandmarks() const {return _number_of_tracked_landmarks;} 57 | const Count& numberOfAvailablePoints() const {return _number_of_available_points;} 58 | 59 | //ds settings 60 | protected: 61 | 62 | const Camera* _camera_left = nullptr; 63 | 64 | //ds image dimensions 65 | int32_t _number_of_rows_image; 66 | int32_t _number_of_cols_image; 67 | 68 | //ds point detection properties 69 | Count _target_number_of_keypoints; 70 | Count _target_number_of_keypoints_per_detector; 71 | Count _number_of_detected_keypoints; 72 | Count _number_of_available_points; 73 | 74 | //ds quick access 75 | real _focal_length_pixels; 76 | real _principal_point_offset_u_pixels; 77 | real _principal_point_offset_v_pixels; 78 | 79 | //! @brief grid of detectors (equally distributed over the image with size=number_of_detectors_per_dimension*number_of_detectors_per_dimension) 80 | cv::Ptr** _detectors = nullptr; 81 | real** _detector_thresholds = nullptr; 82 | 83 | //! @brief number of detectors 84 | //! @brief the same for all image streams 85 | uint32_t _number_of_detectors; 86 | 87 | //! @brief image region for each detector 88 | //! @brief the same for all image streams 89 | cv::Rect** _detector_regions = nullptr; 90 | 91 | //ds descriptor extraction 92 | cv::Ptr _descriptor_extractor; 93 | 94 | //ds feature density regularization 95 | Count _number_of_rows_bin = 0; 96 | Count _number_of_cols_bin = 0; 97 | FramePointMatrix _bin_map_left = nullptr; 98 | 99 | //! @brief feature matching class (maintains features in a 2D lattice corresponding to the image and a vector) 100 | IntensityFeatureMatcher _feature_matcher_left; 101 | std::vector _keypoints_with_descriptors_left; 102 | 103 | //! @brief currently active projection tracking distance (adjusted dynamically at runtime) 104 | int32_t _projection_tracking_distance_pixels = 10; 105 | 106 | //! @brief status 107 | Count _number_of_tracked_landmarks = 0; 108 | 109 | private: 110 | 111 | //ds informative only 112 | CREATE_CHRONOMETER(keypoint_detection) 113 | CREATE_CHRONOMETER(descriptor_extraction) 114 | 115 | }; 116 | } //namespace proslam 117 | -------------------------------------------------------------------------------- /src/framepoint_generation/depth_framepoint_generator.cpp: -------------------------------------------------------------------------------- 1 | #include "depth_framepoint_generator.h" 2 | 3 | namespace proslam { 4 | 5 | DepthFramePointGenerator::DepthFramePointGenerator(DepthFramePointGeneratorParameters* parameters_): BaseFramePointGenerator(parameters_), 6 | _parameters(parameters_), 7 | _camera_right(0) { 8 | LOG_INFO(std::cerr << "DepthFramePointGenerator::DepthFramePointGenerator|constructed" << std::endl) 9 | } 10 | 11 | //ds the stereo camera setup must be provided 12 | void DepthFramePointGenerator::configure(){ 13 | LOG_INFO(std::cerr << "DepthFramePointGenerator::configure|configuring" << std::endl) 14 | assert(_camera_right); 15 | 16 | //ds update base 17 | BaseFramePointGenerator::configure(); 18 | 19 | //ds info 20 | LOG_INFO(std::cerr << "DepthFramePointGenerator::configure|configured" << std::endl) 21 | } 22 | 23 | //ds cleanup of dynamic structures 24 | DepthFramePointGenerator::~DepthFramePointGenerator() { 25 | LOG_INFO(std::cerr << "DepthFramePointGenerator::~DepthFramePointGenerator|destroyed" << std::endl) 26 | } 27 | 28 | void DepthFramePointGenerator::_computeDepthMap(const cv::Mat& right_depth_image) { 29 | if (right_depth_image.type()!=CV_16UC1){ 30 | throw std::runtime_error("depth tracker requires a 16bit mono image to encode depth"); 31 | } 32 | 33 | //ds allocate new space map and initialize fields 34 | _space_map_left_meters.create(_number_of_rows_image,_number_of_cols_image, CV_32FC3); 35 | for (int32_t row = 0; row < _number_of_rows_image; ++row) { 36 | for (int32_t col = 0; col < _number_of_cols_image; ++col) { 37 | _space_map_left_meters.at(row, col) = cv::Vec3f(0,0,_maximum_reliable_depth_far_meters); 38 | } 39 | } 40 | 41 | _row_map.create(_number_of_rows_image,_number_of_cols_image, CV_16SC1); 42 | _row_map=-1; 43 | 44 | _col_map.create(_number_of_rows_image,_number_of_cols_image, CV_16SC1); 45 | _col_map=-1; 46 | 47 | const Matrix3 inverse_camera_matrix_right=_camera_right->cameraMatrix().inverse(); 48 | const float _depth_pixel_to_meters=1e-3; 49 | 50 | TransformMatrix3D right_to_left_transform=_camera_left->robotToCamera()*_camera_right->cameraToRobot(); 51 | for (int32_t r=0; r<_number_of_rows_image; ++r){ 52 | const unsigned short* raw_depth=right_depth_image.ptr(r); 53 | for (int32_t c=0; c<_number_of_cols_image; ++c, raw_depth++){ 54 | if (!*raw_depth) 55 | continue; 56 | // retrieve depth 57 | const float depth_right_meters=(*raw_depth)*_depth_pixel_to_meters; 58 | // retrieve point in right camers, meters 59 | Vector3 point_in_right_camera_meters=inverse_camera_matrix_right*Vector3(c*depth_right_meters, r*depth_right_meters,depth_right_meters); 60 | // map the point to the left camera 61 | Vector3 point_in_left_camera_meters=right_to_left_transform*point_in_right_camera_meters; 62 | // if beyond camera, discard 63 | const real depth_left_meters=point_in_left_camera_meters.z(); 64 | if (depth_left_meters<=0) 65 | continue; 66 | // project to image coordinates 67 | Vector3 point_in_left_camera_pixels=_camera_left->cameraMatrix()*point_in_left_camera_meters; 68 | point_in_left_camera_pixels *= 1./point_in_left_camera_pixels.z(); 69 | 70 | // round to int 71 | const int32_t dest_r=round(point_in_left_camera_pixels.y()); 72 | const int32_t dest_c=round(point_in_left_camera_pixels.x()); 73 | 74 | // if outside skip 75 | if (dest_r<0 || 76 | dest_r>=_number_of_rows_image || 77 | dest_c<0 || 78 | dest_c>=_number_of_cols_image) 79 | continue; 80 | 81 | // do z buffering and update indices 82 | cv::Vec3f& dest_space=_space_map_left_meters.at(dest_r, dest_c); 83 | 84 | if (dest_space[2] > depth_left_meters){ 85 | dest_space=cv::Vec3f(point_in_left_camera_meters.x(), 86 | point_in_left_camera_meters.y(), 87 | point_in_left_camera_meters.z()); 88 | _row_map.at(dest_r, dest_c)=r; 89 | _col_map.at(dest_r, dest_c)=c; 90 | } 91 | } 92 | } 93 | } 94 | 95 | void DepthFramePointGenerator::initialize(Frame* frame_, const bool& extract_features_) { 96 | //ds TODO 97 | } 98 | 99 | //ds computes framepoints stored in a image-like matrix (_framepoints_in_image) for provided stereo images 100 | void DepthFramePointGenerator::compute(Frame* frame_) { 101 | assert(frame_->intensityImageRight().type() == CV_16UC1); 102 | 103 | //ds detect new features 104 | detectKeypoints(frame_->intensityImageLeft(), frame_->keypointsLeft()); 105 | 106 | CHRONOMETER_START(depth_map_generation) 107 | _computeDepthMap(frame_->intensityImageRight()); 108 | CHRONOMETER_STOP(depth_map_generation) 109 | 110 | //ds extract descriptors for detected features 111 | computeDescriptors(frame_->intensityImageLeft(), frame_->keypointsLeft(), frame_->descriptorsLeft()); 112 | 113 | //ds prepare and execute stereo keypoint search 114 | CHRONOMETER_START(depth_assignment) 115 | computeCoordinatesFromDepth(frame_, frame_->keypointsLeft(), frame_->descriptorsLeft()); 116 | CHRONOMETER_STOP(depth_assignment) 117 | } 118 | 119 | //ds computes all potential stereo keypoints (exhaustive in matching distance) and stores them as framepoints (called within compute) 120 | void DepthFramePointGenerator::computeCoordinatesFromDepth(Frame* frame_, std::vector& keypoints_, cv::Mat& descriptors_) { 121 | _number_of_available_points = 0; 122 | frame_->points().resize(keypoints_.size()); 123 | 124 | for (size_t i=0; i(r_left, c_left); 130 | if (p[2]>=_maximum_reliable_depth_far_meters) 131 | continue; 132 | 133 | 134 | cv::KeyPoint keypoint_right=keypoint_left; 135 | keypoint_right.pt.x=_col_map.at(r_left,c_left); 136 | keypoint_right.pt.y=_row_map.at(r_left,c_left); 137 | 138 | const PointCoordinates camera_coordinates=PointCoordinates(p[0], p[1], p[2]); // compute 139 | 140 | //ds add to framepoint map 141 | frame_->points()[_number_of_available_points] = frame_->createFramepoint(keypoint_left, 142 | descriptor_left, 143 | keypoint_right, 144 | descriptor_left, 145 | camera_coordinates); 146 | ++_number_of_available_points; 147 | } 148 | frame_->points().resize(_number_of_available_points); 149 | } 150 | } 151 | -------------------------------------------------------------------------------- /src/framepoint_generation/depth_framepoint_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_framepoint_generator.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class computes potential framepoints in a stereo image pair by triangulation 7 | class DepthFramePointGenerator: public BaseFramePointGenerator { 8 | 9 | //ds object handling 10 | PROSLAM_MAKE_PROCESSING_CLASS(DepthFramePointGenerator) 11 | 12 | //ds functionality 13 | public: 14 | 15 | //ds initializes the framepoint generator (e.g. detects keypoints and computes descriptors) 16 | virtual void initialize(Frame* frame_, const bool& extract_features_ = true); 17 | 18 | //ds computes framepoints stored in a image-like matrix (_framepoints_in_image) for provided stereo images 19 | virtual void compute(Frame* frame_); 20 | 21 | void computeCoordinatesFromDepth(Frame* frame_, std::vector& keypoints_, cv::Mat& descriptors_); 22 | 23 | //ds setters/getters 24 | public: 25 | 26 | inline void setCameraRight(const Camera* camera_right_) {_camera_right = camera_right_;} 27 | 28 | protected: 29 | 30 | void _computeDepthMap(const cv::Mat& right_depth_image); 31 | 32 | //ds settings 33 | protected: 34 | 35 | const Camera* _camera_right; 36 | 37 | const real _maximum_reliable_depth_far_meters = 5; 38 | 39 | //ds inner memory buffers (operated on in compute) 40 | cv::Mat _space_map_left_meters; // xyz coordinates of every pixel of the left image in meters 41 | cv::Mat _row_map; // row index in the depth image(right) corresponding to the pixel ar [r,c] in left image 42 | cv::Mat _col_map; // col index in the depth image(right) corresponding to the pixel ar [r,c] in left image 43 | 44 | private: 45 | 46 | //ds informative only 47 | CREATE_CHRONOMETER(depth_map_generation) 48 | CREATE_CHRONOMETER(depth_assignment) 49 | }; 50 | } 51 | -------------------------------------------------------------------------------- /src/framepoint_generation/intensity_feature_extractor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/definitions.h" 3 | 4 | 5 | 6 | namespace proslam { 7 | 8 | class IntensityFeatureExtractor { 9 | public: 10 | 11 | 12 | protected: 13 | 14 | //! @brief grid of detectors (equally distributed over the image with size=number_of_detectors_per_dimension*number_of_detectors_per_dimension) 15 | cv::Ptr** _detectors = nullptr; 16 | real** _detector_thresholds = nullptr; 17 | 18 | //! @brief number of detectors 19 | //! @brief the same for all image streams 20 | uint32_t _number_of_detectors; 21 | 22 | //! @brief image region for each detector 23 | //! @brief the same for all image streams 24 | cv::Rect** _detector_regions = nullptr; 25 | }; 26 | } //namespace proslam 27 | -------------------------------------------------------------------------------- /src/framepoint_generation/intensity_feature_matcher.cpp: -------------------------------------------------------------------------------- 1 | #include "intensity_feature_matcher.h" 2 | #include "types/frame_point.h" 3 | 4 | 5 | 6 | namespace proslam { 7 | 8 | IntensityFeatureMatcher::IntensityFeatureMatcher() { 9 | feature_vector.clear(); 10 | LOG_INFO(std::cerr << "IntensityFeatureMatcher::IntensityFeatureMatcher|constructed" << std::endl) 11 | } 12 | 13 | IntensityFeatureMatcher::~IntensityFeatureMatcher() { 14 | LOG_INFO(std::cerr << "IntensityFeatureMatcher::~IntensityFeatureMatcher|destroying" << std::endl) 15 | for (int32_t r = 0; r < number_of_rows; ++r) { 16 | delete[] feature_lattice[r]; 17 | } 18 | delete[] feature_lattice; 19 | for (IntensityFeature* feature: feature_vector) { 20 | delete feature; 21 | } 22 | feature_vector.clear(); 23 | LOG_INFO(std::cerr << "IntensityFeatureMatcher::~IntensityFeatureMatcher|destroyed" << std::endl) 24 | } 25 | 26 | void IntensityFeatureMatcher::configure(const int32_t& rows_, const int32_t& cols_) { 27 | LOG_INFO(std::cerr << "IntensityFeatureMatcher::configure|configuring" << std::endl) 28 | if (rows_ <= 0 || cols_ <= 0) { 29 | throw std::runtime_error("KeypointWithDescriptorLattice::configure|invalid image dimensions"); 30 | } 31 | if (feature_lattice) { 32 | throw std::runtime_error("KeypointWithDescriptorLattice::configure|lattice already allocated"); 33 | } 34 | 35 | //ds initialize empty lattice 36 | feature_lattice = new IntensityFeature**[rows_]; 37 | for (int32_t r = 0; r < rows_; ++r) { 38 | feature_lattice[r] = new IntensityFeature*[cols_]; 39 | for (int32_t c = 0; c < cols_; ++c) { 40 | feature_lattice[r][c] = nullptr; 41 | } 42 | } 43 | number_of_rows = rows_; 44 | number_of_cols = cols_; 45 | LOG_INFO(std::cerr << "IntensityFeatureMatcher::configure|configured" << std::endl) 46 | } 47 | 48 | void IntensityFeatureMatcher::setFeatures(const std::vector& keypoints_, const cv::Mat& descriptors_) { 49 | if (keypoints_.size() != static_cast(descriptors_.rows)) { 50 | throw std::runtime_error("KeypointWithDescriptorLattice::setFeatures|mismatching keypoints and descriptor numbers"); 51 | } 52 | 53 | //ds clear the lattice - freeing remaining features 54 | for (int32_t r = 0; r < number_of_rows; ++r) { 55 | for (int32_t c = 0; c < number_of_cols; ++c) { 56 | feature_lattice[r][c] = nullptr; 57 | } 58 | } 59 | for (IntensityFeature* feature: feature_vector) { 60 | delete feature; 61 | } 62 | 63 | //ds fill in features 64 | feature_vector.resize(keypoints_.size()); 65 | for (uint32_t index = 0; index < keypoints_.size(); ++index) { 66 | IntensityFeature* feature = new IntensityFeature(keypoints_[index], descriptors_.row(index), index); 67 | feature_vector[index] = feature; 68 | feature_lattice[feature->row][feature->col] = feature; 69 | } 70 | } 71 | 72 | void IntensityFeatureMatcher::sortFeatureVector() { 73 | std::sort(feature_vector.begin(), feature_vector.end(), [](const IntensityFeature* a_, const IntensityFeature* b_){ 74 | return ((a_->row < b_->row) || (a_->row == b_->row && a_->col < b_->col)); 75 | }); 76 | } 77 | 78 | IntensityFeature* IntensityFeatureMatcher::getMatchingFeatureInRectangularRegion(const int32_t& row_reference_, 79 | const int32_t& col_reference_, 80 | const cv::Mat& descriptor_reference_, 81 | const int32_t& row_start_point, 82 | const int32_t& row_end_point, 83 | const int32_t& col_start_point, 84 | const int32_t& col_end_point, 85 | const real& maximum_descriptor_distance_tracking_, 86 | const bool track_by_appearance_, 87 | real& descriptor_distance_best_) { 88 | descriptor_distance_best_ = maximum_descriptor_distance_tracking_; 89 | int32_t row_best = -1; 90 | int32_t col_best = -1; 91 | 92 | //ds locate best match in appearance 93 | if (track_by_appearance_) { 94 | for (int32_t row = row_start_point; row < row_end_point; ++row) { 95 | for (int32_t col = col_start_point; col < col_end_point; ++col) { 96 | if (feature_lattice[row][col]) { 97 | const real descriptor_distance = cv::norm(descriptor_reference_, 98 | feature_lattice[row][col]->descriptor, 99 | SRRG_PROSLAM_DESCRIPTOR_NORM); 100 | 101 | if (descriptor_distance < descriptor_distance_best_) { 102 | descriptor_distance_best_ = descriptor_distance; 103 | row_best = row; 104 | col_best = col; 105 | } 106 | } 107 | } 108 | } 109 | 110 | //ds locate best match in projection error, within maximum appearance distance 111 | } else { 112 | uint32_t projection_distance_pixels_best = 10000; 113 | for (int32_t row = row_start_point; row < row_end_point; ++row) { 114 | for (int32_t col = col_start_point; col < col_end_point; ++col) { 115 | if (feature_lattice[row][col]) { 116 | const real descriptor_distance = cv::norm(descriptor_reference_, 117 | feature_lattice[row][col]->descriptor, 118 | SRRG_PROSLAM_DESCRIPTOR_NORM); 119 | if (descriptor_distance < maximum_descriptor_distance_tracking_) { 120 | 121 | //ds compute projection distance 122 | const uint32_t row_distance_pixels = row_reference_-row; 123 | const uint32_t col_distance_pixels = col_reference_-col; 124 | const uint32_t projection_distance_pixels = row_distance_pixels*row_distance_pixels+col_distance_pixels*col_distance_pixels; 125 | 126 | //ds if better than best so far 127 | if (projection_distance_pixels < projection_distance_pixels_best) { 128 | projection_distance_pixels_best = projection_distance_pixels; 129 | descriptor_distance_best_ = descriptor_distance; 130 | row_best = row; 131 | col_best = col; 132 | } 133 | } 134 | } 135 | } 136 | } 137 | } 138 | 139 | //ds if we found a match 140 | if (row_best != -1) { 141 | return feature_lattice[row_best][col_best]; 142 | } else { 143 | return nullptr; 144 | } 145 | } 146 | 147 | void IntensityFeatureMatcher::prune(const std::set& matched_indices_) { 148 | 149 | //ds remove matched indices from candidate pools 150 | size_t number_of_unmatched_elements = 0; 151 | for (size_t index = 0; index < feature_vector.size(); ++index) { 152 | 153 | //ds if we haven't matched this index yet 154 | if (matched_indices_.count(index) == 0) { 155 | 156 | //ds keep the element (this operation is not problemenatic since we do not loop reversely here) 157 | feature_vector[number_of_unmatched_elements] = feature_vector[index]; 158 | ++number_of_unmatched_elements; 159 | } 160 | } 161 | feature_vector.resize(number_of_unmatched_elements); 162 | } 163 | } //namespace proslam 164 | -------------------------------------------------------------------------------- /src/framepoint_generation/intensity_feature_matcher.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "intensity_feature_extractor.h" 3 | #include "types/frame_point.h" 4 | 5 | 6 | 7 | namespace proslam { 8 | 9 | //! @struct support structure 10 | class IntensityFeatureMatcher { 11 | public: 12 | 13 | IntensityFeatureMatcher(); 14 | ~IntensityFeatureMatcher(); 15 | 16 | public: 17 | 18 | //ds configure matcher 19 | void configure(const int32_t& rows_, const int32_t& cols_); 20 | 21 | //ds create features from keypoints and descriptors 22 | void setFeatures(const std::vector& keypoints_, const cv::Mat& descriptors_); 23 | 24 | //ds sort all input vectors by ascending row positions (preparation for stereo matching) 25 | void sortFeatureVector(); 26 | 27 | //ds performs a local search in a rectangular area on the feature lattice 28 | IntensityFeature* getMatchingFeatureInRectangularRegion(const int32_t& row_reference_, 29 | const int32_t& col_reference_, 30 | const cv::Mat& descriptor_reference_, 31 | const int32_t& row_start_point, 32 | const int32_t& row_end_point, 33 | const int32_t& col_start_point, 34 | const int32_t& col_end_point, 35 | const real& maximum_descriptor_distance_tracking_, 36 | const bool track_by_appearance_, 37 | real& descriptor_distance_best_); 38 | 39 | //ds prunes features from feature vector if existing 40 | void prune(const std::set& matched_indices_); 41 | 42 | //ds attributes 43 | public: 44 | 45 | int32_t number_of_rows = 0; 46 | int32_t number_of_cols = 0; 47 | IntensityFeaturePointerVector feature_vector; 48 | IntensityFeature*** feature_lattice = nullptr; 49 | 50 | }; 51 | } //namespace proslam 52 | -------------------------------------------------------------------------------- /src/framepoint_generation/stereo_framepoint_generator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_framepoint_generator.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class computes potential framepoints in a stereo image pair by triangulation 7 | class StereoFramePointGenerator : public BaseFramePointGenerator { 8 | 9 | //ds object handling 10 | PROSLAM_MAKE_PROCESSING_CLASS(StereoFramePointGenerator) 11 | 12 | //ds functionality 13 | public: 14 | 15 | //ds initializes the framepoint generator (e.g. detects keypoints and computes descriptors in the left and right camera image) 16 | virtual void initialize(Frame* frame_, const bool& extract_features_ = true); 17 | 18 | //! @brief computes framepoints based on exhaustive, rigid stereo matching on multiple epipolar lines without prior 19 | //! @param[in, out] frame_ frame that will be filled with framepoints 20 | virtual void compute(Frame* frame_); 21 | 22 | //! @brief computes first tracks between previous framepoints, estimates the projection error 23 | //! @brief and uses the prior on the right camera for fast and reliable stereo matching 24 | //! @param[in, out] frame_ frame that will be filled with framepoints and tracks 25 | //! @param[in] frame_previous_ previous frame that contains valid framepoints on which we will track 26 | //! @param[in] camera_left_previous_in_current_ the relative camera motion guess between frame_ and frame_previous_ 27 | //! @param[out] previous_points_without_tracks_ lost points 28 | void track(Frame* frame_, 29 | Frame* frame_previous_, 30 | const TransformMatrix3D& camera_left_previous_in_current_, 31 | FramePointPointerVector& previous_framepoints_without_tracks_, 32 | const bool track_by_appearance_ = true) override; 33 | 34 | //ds computes 3D position of a stereo keypoint pair in the keft camera frame 35 | const PointCoordinates getPointInLeftCamera(const cv::Point2f& image_coordinates_left_, const cv::Point2f& image_coordinates_right_) const; 36 | 37 | //ds setters/getters 38 | public: 39 | 40 | inline void setCameraRight(const Camera* camera_right_) {_camera_right = camera_right_;} 41 | const real& meanTriangulationSuccessRatio() const {return _mean_triangulation_success_ratio;} 42 | 43 | //ds settings 44 | protected: 45 | 46 | //! @brief right camera handle 47 | const Camera* _camera_right = 0; 48 | 49 | //! @brief derived triangulation properties - updated to current camera configuration before each compute 50 | Vector3 _baseline = Vector3::Zero(); 51 | real _baseline_pixelsmeters = 0; 52 | real _baseline_meters = 0; 53 | real _f_x = 0; 54 | real _f_y = 0; 55 | real _c_x = 0; 56 | real _c_y = 0; 57 | real _b_x = 0; 58 | 59 | //! @brief current epipolar search range 60 | int32_t _maximum_epipolar_search_offset_pixels = 0; 61 | 62 | //! @brief current triangulation distance 63 | real _current_maximum_descriptor_distance_triangulation = 0.1*SRRG_PROSLAM_DESCRIPTOR_SIZE_BITS; 64 | 65 | //! @brief information only: average triangulation success ratio 66 | real _mean_triangulation_success_ratio = 1; 67 | Count _number_of_triangulations = 1; 68 | 69 | //! @brief horizontal epipolar stereo matching search offsets (to consider for stereo matching) 70 | std::vector _epipolar_search_offsets_pixel; 71 | 72 | //! @brief feature matching class (maintains features in a 2D lattice corresponding to the image and a vector) 73 | IntensityFeatureMatcher _feature_matcher_right; 74 | 75 | private: 76 | 77 | //ds informative only 78 | CREATE_CHRONOMETER(point_triangulation) 79 | }; 80 | } 81 | -------------------------------------------------------------------------------- /src/map_optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_map_optimization_library 2 | graph_optimizer.cpp 3 | ) 4 | 5 | target_link_libraries(srrg_proslam_map_optimization_library 6 | srrg_proslam_types_library 7 | ${g2o_LIBRARIES} 8 | ${CSPARSE_LIBRARY} 9 | ${CHOLMOD_LIBRARY} 10 | ) 11 | -------------------------------------------------------------------------------- /src/map_optimization/graph_optimizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //ds g2o 4 | #include "g2o/core/optimizable_graph.h" 5 | #include "g2o/core/sparse_optimizer.h" 6 | #include "g2o/core/block_solver.h" 7 | #include "g2o/core/factory.h" 8 | #include "g2o/core/optimization_algorithm_factory.h" 9 | #include "g2o/solvers/csparse/linear_solver_csparse.h" 10 | #include "g2o/solvers/cholmod/linear_solver_cholmod.h" 11 | #include "g2o/types/slam3d/types_slam3d.h" 12 | #include "g2o/core/optimization_algorithm_gauss_newton.h" 13 | #include "g2o/core/optimization_algorithm_levenberg.h" 14 | 15 | //ds proslam 16 | #include "types/world_map.h" 17 | #include "relocalization/closure.h" 18 | 19 | namespace proslam { 20 | 21 | //ds this class optimizes the SLAM pose graph by considering pose measurements only, connected landmarks are moved rigidly after the optimization 22 | class GraphOptimizer { 23 | 24 | //ds exported data types 25 | public: 26 | 27 | //ds available block solver types (the less numbers, the faster it is - variable being the slowest) 28 | typedef g2o::BlockSolver> BlockSolverVariable; 29 | typedef g2o::BlockSolver> BlockSolver6x3; 30 | 31 | //ds available solvers 32 | typedef g2o::LinearSolverCSparse LinearSolverCSparseVariable; 33 | typedef g2o::LinearSolverCholmod LinearSolverCholmodVariable; 34 | typedef g2o::LinearSolverCSparse LinearSolverCSparse6x3; 35 | typedef g2o::LinearSolverCholmod LinearSolverCholmod6x3; 36 | 37 | //ds available optimization algorithm 38 | typedef g2o::OptimizationAlgorithmGaussNewton OptimizerGaussNewton; 39 | typedef g2o::OptimizationAlgorithmLevenberg OptimizerLevenberg; 40 | 41 | //! @brief g2o parameter identifiers 42 | enum G2oParameter { 43 | WORLD_OFFSET = 0, 44 | CAMERA_LEFT = 1, 45 | CAMERA_RIGHT = 2, 46 | OFFSET_IMUtoLEFT = 3 47 | }; 48 | 49 | //ds object management 50 | PROSLAM_MAKE_PROCESSING_CLASS(GraphOptimizer) 51 | 52 | //ds interface 53 | public: 54 | 55 | //! @brief saves a g2o graph of the provided world map to a file 56 | //! @param[in] world_map_ world map for which the pose graph is constructed 57 | //! @param[in] file_name_ desired file name for the g2o outfile 58 | void writePoseGraphToFile(const WorldMap* world_map_, const std::string& file_name_) const; 59 | 60 | //! @brief adds a new frame to the pose graph 61 | //! @param[in] frame_ the frame to add 62 | void addFrame(Frame* frame_); 63 | 64 | //! @brief adds a new frame to the pose graph with all connected landmarks 65 | //! @param[in] frame_ the frame to add including its captured landmarks 66 | void addFrameWithLandmarks(Frame* frame_); 67 | 68 | //! @brief triggers an adjustment of poses only 69 | //! @param[in] world_map_ map in which the optimization takes place 70 | void optimizeFrames(WorldMap* world_map_); 71 | 72 | //! @brief triggers a full bundle adjustment optimization of the current pose graph 73 | //! @param[in] world_map_ map in which the optimization takes place 74 | void optimizeFramesWithLandmarks(WorldMap* world_map_); 75 | 76 | //ds getters/setters 77 | public: 78 | 79 | const Count numberOfOptimizations() const {return _number_of_optimizations;} 80 | 81 | //ds g2o wrapper functions 82 | protected: 83 | 84 | void _setPoseEdge(g2o::OptimizableGraph* optimizer_, 85 | g2o::OptimizableGraph::Vertex* vertex_from_, 86 | g2o::OptimizableGraph::Vertex* vertex_to_, 87 | const TransformMatrix3D& transform_from_to_, 88 | const real& information_factor_, 89 | const bool& free_translation_ = true, 90 | const bool& enable_robust_kernel_ = false) const; 91 | 92 | void _setPointEdge(g2o::OptimizableGraph* optimizer_, 93 | g2o::VertexSE3* vertex_frame_, 94 | g2o::VertexPointXYZ* vertex_landmark_, 95 | const PointCoordinates& framepoint_robot_coordinates, 96 | const real& information_factor_) const; 97 | 98 | //ds attributes 99 | protected: 100 | 101 | //! @brief g2o optimizer (holding the pose graph) 102 | g2o::SparseOptimizer* _optimizer; 103 | 104 | //! @brief last frame vertex added (to be locked for optimization) 105 | g2o::VertexSE3* _vertex_frame_last_added; 106 | 107 | //! @brief bookkeeping: added frames 108 | std::map _frames_in_pose_graph; 109 | 110 | //! @brief bookkeeping: added local maps 111 | std::map _local_maps_in_graph; 112 | 113 | //! @brief bookkeeping: added landmarks 114 | std::map _landmarks_in_pose_graph; 115 | 116 | //ds informative only 117 | CREATE_CHRONOMETER(addition) 118 | CREATE_CHRONOMETER(optimization) 119 | Count _number_of_optimizations = 0; 120 | }; 121 | } 122 | -------------------------------------------------------------------------------- /src/position_tracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_position_tracking_library 2 | base_tracker.cpp 3 | stereo_tracker.cpp 4 | depth_tracker.cpp 5 | ) 6 | 7 | target_link_libraries(srrg_proslam_position_tracking_library 8 | srrg_proslam_aligners_library 9 | srrg_proslam_framepoint_generation_library 10 | ) 11 | -------------------------------------------------------------------------------- /src/position_tracking/base_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "framepoint_generation/base_framepoint_generator.h" 3 | #include "aligners/base_frame_aligner.h" 4 | #include "types/world_map.h" 5 | 6 | namespace proslam { 7 | 8 | //ds this class processes two subsequent Frames and establishes Framepoint correspondences (tracks) based on the corresponding images 9 | class BaseTracker { 10 | 11 | //ds object handling 12 | PROSLAM_MAKE_PROCESSING_CLASS(BaseTracker) 13 | 14 | //ds functionality 15 | public: 16 | 17 | //! @brief creates a new Frame for the given images, retrieves the correspondences relative to the previous Frame, optimizes the current frame pose and updates landmarks 18 | virtual void compute(); 19 | 20 | //! @breaks the track at the current frame 21 | //! @param[in] frame_ target frame to break the track at 22 | void breakTrack(Frame* frame_); 23 | 24 | //ds getters/setters 25 | public: 26 | 27 | const uint64_t& numberOfRecursiveRegistrations() const {return _number_of_recursive_registrations;} 28 | void setCameraLeft(const Camera* camera_left_) {_camera_left = camera_left_; _has_odometry = false;} 29 | void setOdometry(const TransformMatrix3D& odometry_) {_odometry = odometry_; _has_odometry = true;} 30 | void setAligner(BaseFrameAligner* pose_optimizer_) {_pose_optimizer = pose_optimizer_;} 31 | void setFramePointGenerator(BaseFramePointGenerator * framepoint_generator_) {_framepoint_generator = framepoint_generator_;} 32 | void setWorldMap(WorldMap* context_) {_context = context_;} 33 | void setIntensityImageLeft(const cv::Mat& intensity_image_left_) {_intensity_image_left = intensity_image_left_;} 34 | BaseFrameAligner* aligner() {return _pose_optimizer;} 35 | void setMotionPreviousToCurrent(const TransformMatrix3D& motion_previous_to_current_) {_previous_to_current_camera = motion_previous_to_current_;} 36 | BaseFramePointGenerator* framepointGenerator() {return _framepoint_generator;} 37 | const BaseFramePointGenerator* framepointGenerator() const {return _framepoint_generator;} 38 | const Count totalNumberOfTrackedPoints() const {return _total_number_of_tracked_points;} 39 | const Count totalNumberOfLandmarks() const {return _total_number_of_landmarks;} 40 | const real meanTrackingRatio() const {return _mean_tracking_ratio;} 41 | const real meanNumberOfKeypoints() const {return _mean_number_of_keypoints;} 42 | const real meanNumberOfFramepoints() const {return _mean_number_of_framepoints;} 43 | 44 | //ds helpers 45 | protected: 46 | 47 | //ds retrieves framepoint correspondences between previous and current frame 48 | void _track(Frame* previous_frame_, 49 | Frame* current_frame_, 50 | const TransformMatrix3D& previous_to_current_, 51 | const bool& track_by_appearance_ = false); 52 | 53 | //! @brief recursive registration method, that calls track framepoints with different parameters upon failure 54 | //! @param [in] frame_previous_ the previous frame 55 | //! @param [in] frame_current_ the current frame to align against the previous frame 56 | void _registerRecursive(Frame* frame_previous_, 57 | Frame* frame_current_, 58 | TransformMatrix3D previous_to_current_, 59 | const Count& recursion_ = 0); 60 | 61 | //ds prunes invalid tracks after pose optimization 62 | void _prunePoints(Frame* frame_); 63 | 64 | //ds updates existing or creates new landmarks for framepoints of the provided frame 65 | void _updatePoints(WorldMap* context_, Frame* frame_); 66 | 67 | //ds attempts to recover framepoints in the current image using the more precise pose estimate, retrieved after pose optimization 68 | virtual void _recoverPoints(Frame* current_frame_) = 0; 69 | 70 | //ds creates a frame, which is filled by calling the framepoint generator 71 | virtual Frame* _createFrame() = 0; 72 | 73 | //ds attributes 74 | protected: 75 | 76 | //ds tracker state 77 | Frame::Status _status = Frame::Localizing; 78 | Frame::Status _status_previous = Frame::Localizing; 79 | uint64_t _number_of_recursive_registrations = 0; 80 | 81 | //ds running variables and buffered values 82 | Count _number_of_tracked_landmarks = 0; 83 | Count _number_of_potential_points = 0; 84 | Count _number_of_tracked_points = 0; 85 | Count _number_of_tracked_landmarks_previous = 0; 86 | Count _number_of_active_landmarks = 0; 87 | real _tracking_ratio = 0; 88 | real _mean_tracking_ratio = 0; 89 | Count _number_of_tracked_frames = 0; 90 | const Camera* _camera_left = nullptr; 91 | 92 | //! @brief currently active projection tracking distance (adjusted dynamically at runtime) 93 | int32_t _projection_tracking_distance_pixels = 0; 94 | 95 | //gg working elements 96 | cv::Mat _intensity_image_left; 97 | WorldMap* _context = nullptr; 98 | 99 | //gg processing objects 100 | BaseFrameAligner* _pose_optimizer = nullptr; 101 | BaseFramePointGenerator* _framepoint_generator = nullptr; 102 | 103 | //! @brief position tracking bookkeeping 104 | TransformMatrix3D _previous_to_current_camera = TransformMatrix3D::Identity(); 105 | 106 | //ds framepoint track recovery 107 | Count _number_of_lost_points = 0; 108 | Count _number_of_recovered_points = 0; 109 | FramePointPointerVector _lost_points; 110 | 111 | //ds stats only 112 | real _mean_number_of_keypoints = 0; 113 | real _mean_number_of_framepoints = 0; 114 | 115 | private: 116 | 117 | //ds informative only 118 | CREATE_CHRONOMETER(tracking) 119 | CREATE_CHRONOMETER(track_creation) 120 | CREATE_CHRONOMETER(pose_optimization) 121 | CREATE_CHRONOMETER(landmark_optimization) 122 | CREATE_CHRONOMETER(point_recovery) 123 | Count _total_number_of_tracked_points = 0; 124 | Count _total_number_of_landmarks = 0; 125 | bool _has_odometry; 126 | TransformMatrix3D _odometry; 127 | TransformMatrix3D _previous_odometry; 128 | }; 129 | } 130 | -------------------------------------------------------------------------------- /src/position_tracking/depth_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "depth_tracker.h" 2 | 3 | namespace proslam { 4 | using namespace srrg_core; 5 | 6 | DepthTracker::DepthTracker(DepthTrackerParameters* parameters_): BaseTracker(parameters_), 7 | _parameters(parameters_) { 8 | LOG_INFO(std::cerr << "DepthTracker::DepthTracker|constructed" << std::endl) 9 | } 10 | 11 | void DepthTracker::configure() { 12 | LOG_INFO(std::cerr << "DepthTracker::configure|configuring" << std::endl) 13 | BaseTracker::configure(); 14 | assert(_depth_camera); 15 | _depth_framepoint_generator = dynamic_cast(_framepoint_generator); 16 | assert(_depth_framepoint_generator); 17 | LOG_INFO(std::cerr << "DepthTracker::configure|configured" << std::endl) 18 | } 19 | 20 | DepthTracker::~DepthTracker() { 21 | LOG_INFO(std::cerr << "DepthTracker::~DepthTracker|destroyed" << std::endl) 22 | } 23 | 24 | Frame* DepthTracker::_createFrame(){ 25 | Frame* current_frame = _context->createFrame(_context->robotToWorld()); 26 | current_frame->setCameraLeft(_camera_left); 27 | current_frame->setIntensityImageLeft(_intensity_image_left); 28 | current_frame->setCameraRight(_depth_camera); 29 | current_frame->setIntensityImageRight(_depth_image); 30 | return current_frame; 31 | } 32 | 33 | void DepthTracker::compute() { 34 | BaseTracker::compute(); 35 | } 36 | 37 | void DepthTracker::_recoverPoints(Frame* current_frame_) { 38 | return; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /src/position_tracking/depth_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "position_tracking/base_tracker.h" 3 | #include "framepoint_generation/depth_framepoint_generator.h" 4 | 5 | namespace proslam { 6 | 7 | //ds this class processes two subsequent Frames and establishes Framepoint correspondences (tracks) based on the corresponding images 8 | class DepthTracker: public BaseTracker { 9 | 10 | //ds object management 11 | PROSLAM_MAKE_PROCESSING_CLASS(DepthTracker) 12 | 13 | //ds functionality 14 | public: 15 | 16 | virtual void compute(); 17 | 18 | //ds setters/getters 19 | public: 20 | 21 | void setDepthCamera(const Camera* depth_camera_) {_depth_camera = depth_camera_;} 22 | void setDepthImage(const cv::Mat& depth_image_) {_depth_image = depth_image_;} 23 | 24 | //ds helpers 25 | protected: 26 | 27 | //gg 28 | virtual Frame* _createFrame(); 29 | 30 | //ds attempts to recover framepoints in the current image using the more precise pose estimate, retrieved after pose optimization 31 | virtual void _recoverPoints(Frame* current_frame_); 32 | 33 | //ds attributes 34 | protected: 35 | 36 | //ds configuration 37 | const Camera* _depth_camera = nullptr; 38 | 39 | //ds processing 40 | cv::Mat _depth_image; 41 | 42 | //ds specified generator instance 43 | DepthFramePointGenerator* _depth_framepoint_generator = nullptr; 44 | }; 45 | } 46 | -------------------------------------------------------------------------------- /src/position_tracking/stereo_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "stereo_tracker.h" 2 | 3 | namespace proslam { 4 | using namespace srrg_core; 5 | 6 | //ds the tracker assumes a constant stereo camera configuration 7 | StereoTracker::StereoTracker(StereoTrackerParameters* parameters_): BaseTracker(parameters_), 8 | _parameters(parameters_) { 9 | LOG_INFO(std::cerr << "StereoTracker::StereoTracker|constructed" << std::endl) 10 | } 11 | 12 | void StereoTracker::configure() { 13 | LOG_INFO(std::cerr << "StereoTracker::configure|configuring" << std::endl) 14 | assert(_camera_right); 15 | BaseTracker::configure(); 16 | _stereo_framepoint_generator = dynamic_cast(_framepoint_generator); 17 | assert(_stereo_framepoint_generator); 18 | LOG_INFO(std::cerr << "StereoTracker::configure|configured" << std::endl) 19 | } 20 | 21 | //ds dynamic cleanup 22 | StereoTracker::~StereoTracker() { 23 | LOG_INFO(std::cerr << "StereoTracker::~StereoTracker|destroyed" << std::endl) 24 | } 25 | 26 | Frame* StereoTracker::_createFrame(){ 27 | Frame* current_frame = _context->createFrame(_context->robotToWorld()); 28 | current_frame->setCameraLeft(_camera_left); 29 | current_frame->setIntensityImageLeft(_intensity_image_left); 30 | current_frame->setCameraRight(_camera_right); 31 | current_frame->setIntensityImageRight(_intensity_image_right); 32 | current_frame->setRobotToWorld(_context->robotToWorld()); //ds TODO remove 33 | return current_frame; 34 | } 35 | 36 | //ds creates a new Frame for the given images, retrieves the correspondences relative to the previous Frame, optimizes the current frame pose and updates landmarks 37 | void StereoTracker::compute() { 38 | BaseTracker::compute(); 39 | } 40 | 41 | //ds attempts to recover framepoints in the current image using the more precise pose estimate, retrieved after pose optimization 42 | void StereoTracker::_recoverPoints(Frame* current_frame_) { 43 | 44 | //ds precompute transforms 45 | const TransformMatrix3D world_to_camera_left = current_frame_->worldToCameraLeft(); 46 | const CameraMatrix& camera_calibration_matrix = _camera_left->cameraMatrix(); 47 | const Vector3 baseline_homogeneous = _camera_right->baselineHomogeneous(); 48 | 49 | //ds obtain currently active tracking distance 50 | const real maximum_descriptor_distance = _framepoint_generator->parameters()->matching_distance_tracking_threshold; 51 | 52 | //ds buffers 53 | const cv::Mat& intensity_image_left = current_frame_->intensityImageLeft(); 54 | const cv::Mat& intensity_image_right = current_frame_->intensityImageRight(); 55 | std::vector keypoint_buffer_left(1); 56 | std::vector keypoint_buffer_right(1); 57 | 58 | //ds recover lost landmarks 59 | Index index_lost_point_recovered = _number_of_tracked_points; 60 | current_frame_->points().resize(_number_of_tracked_points+_number_of_lost_points); 61 | for (FramePoint* point_previous: _lost_points) { 62 | 63 | //ds skip non landmarks for now (TODO parametrize) 64 | if (!point_previous->landmark()) { 65 | continue; 66 | } 67 | 68 | //ds get point into current camera - based on last track 69 | PointCoordinates point_in_camera_homogeneous(Vector3::Zero()); 70 | 71 | //ds if we have a landmark at hand 72 | if (point_previous->landmark()) { 73 | point_previous->landmark()->incrementNumberOfRecoveries(); 74 | 75 | //ds get point in camera frame based on landmark coordinates 76 | point_in_camera_homogeneous = world_to_camera_left*point_previous->landmark()->coordinates(); 77 | } else { 78 | 79 | //ds get point in camera frame based on point coordinates 80 | point_in_camera_homogeneous = world_to_camera_left*point_previous->worldCoordinates(); 81 | } 82 | 83 | //ds obtain point projection on camera image plane 84 | PointCoordinates point_in_image_left = camera_calibration_matrix*point_in_camera_homogeneous; 85 | PointCoordinates point_in_image_right = point_in_image_left+baseline_homogeneous; 86 | 87 | //ds normalize point and update prediction based on landmark position: LEFT 88 | point_in_image_left /= point_in_image_left.z(); 89 | point_in_image_right /= point_in_image_right.z(); 90 | 91 | //ds check for invalid projections 92 | if (point_in_image_left.x() < 0 || point_in_image_left.x() > _camera_left->numberOfImageCols() || 93 | point_in_image_right.x() < 0 || point_in_image_right.x() > _camera_left->numberOfImageCols()|| 94 | point_in_image_left.y() < 0 || point_in_image_left.y() > _camera_left->numberOfImageRows() ) { 95 | 96 | //ds out of FOV 97 | continue; 98 | } 99 | assert(point_in_image_left.y() == point_in_image_right.y()); 100 | 101 | //ds set projections - at subpixel accuarcy 102 | const cv::Point2f projection_left(point_in_image_left.x(), point_in_image_left.y()); 103 | const cv::Point2f projection_right(point_in_image_right.x(), point_in_image_right.y()); 104 | 105 | //ds this can be moved outside of the loop if keypoint sizes are constant 106 | const float regional_border_center = 5*point_previous->keypointLeft().size; 107 | const cv::Point2f offset_keypoint_half(regional_border_center, regional_border_center); 108 | const float regional_full_height = regional_border_center+regional_border_center+1; 109 | 110 | //ds if available search range is insufficient 111 | if (projection_left.x <= regional_border_center+1 || 112 | projection_left.x >= _camera_left->numberOfImageCols()-regional_border_center-1 || 113 | projection_left.y <= regional_border_center+1 || 114 | projection_left.y >= _camera_left->numberOfImageRows()-regional_border_center-1 || 115 | projection_right.x <= regional_border_center+1 || 116 | projection_right.x >= _camera_left->numberOfImageCols()-regional_border_center-1) { 117 | 118 | //ds skip complete tracking 119 | continue; 120 | } 121 | 122 | //ds left search regions 123 | const cv::Point2f corner_left(projection_left-offset_keypoint_half); 124 | const cv::Rect_ region_of_interest_left(corner_left.x, corner_left.y, regional_full_height, regional_full_height); 125 | 126 | //ds extract descriptors at this position: LEFT 127 | keypoint_buffer_left[0] = point_previous->keypointLeft(); 128 | keypoint_buffer_left[0].pt = offset_keypoint_half; 129 | cv::Mat descriptor_left; 130 | const cv::Mat roi_left(intensity_image_left(region_of_interest_left)); 131 | _framepoint_generator->descriptorExtractor()->compute(roi_left, keypoint_buffer_left, descriptor_left); 132 | 133 | //ds if no descriptor could be computed 134 | if (descriptor_left.rows == 0) { 135 | continue; 136 | } 137 | 138 | //ds if descriptor distance is to high 139 | if (cv::norm(point_previous->descriptorLeft(), descriptor_left, SRRG_PROSLAM_DESCRIPTOR_NORM) > maximum_descriptor_distance) { 140 | continue; 141 | } 142 | keypoint_buffer_left[0].pt += corner_left; 143 | 144 | //ds right search region 145 | const cv::Point2f corner_right(projection_right-offset_keypoint_half); 146 | const cv::Rect_ region_of_interest_right(corner_right.x, corner_right.y, regional_full_height, regional_full_height); 147 | 148 | //ds extract descriptors at this position: RIGHT 149 | keypoint_buffer_right[0] = point_previous->keypointRight(); 150 | keypoint_buffer_right[0].pt = offset_keypoint_half; 151 | cv::Mat descriptor_right; 152 | const cv::Mat roi_right(intensity_image_right(region_of_interest_right)); 153 | _framepoint_generator->descriptorExtractor()->compute(roi_right, keypoint_buffer_right, descriptor_right); 154 | 155 | //ds if no descriptor could be computed 156 | if (descriptor_right.rows == 0) { 157 | continue; 158 | } 159 | 160 | //ds if descriptor distance is to high 161 | if (cv::norm(point_previous->descriptorRight(), descriptor_right, SRRG_PROSLAM_DESCRIPTOR_NORM) > maximum_descriptor_distance) { 162 | continue; 163 | } 164 | keypoint_buffer_right[0].pt += corner_right; 165 | 166 | //ds skip points with insufficient stereo disparity 167 | if (keypoint_buffer_left[0].pt.x-keypoint_buffer_right[0].pt.x < _stereo_framepoint_generator->parameters()->minimum_disparity_pixels) { 168 | continue; 169 | } 170 | 171 | //ds allocate a new point connected to the previous one 172 | FramePoint* current_point = current_frame_->createFramepoint(keypoint_buffer_left[0], 173 | descriptor_left, 174 | keypoint_buffer_right[0], 175 | descriptor_right, 176 | _stereo_framepoint_generator->getPointInLeftCamera(keypoint_buffer_left[0].pt, keypoint_buffer_right[0].pt), 177 | point_previous); 178 | 179 | //ds set the point to the control structure 180 | current_frame_->points()[index_lost_point_recovered] = current_point; 181 | ++index_lost_point_recovered; 182 | } 183 | _number_of_recovered_points = index_lost_point_recovered-_number_of_tracked_points; 184 | _number_of_tracked_points = index_lost_point_recovered; 185 | current_frame_->points().resize(_number_of_tracked_points); 186 | LOG_DEBUG(std::cerr << "StereoTracker::_recoverPoints|recovered points: " << _number_of_recovered_points << "/" << _number_of_lost_points << std::endl) 187 | } 188 | } 189 | -------------------------------------------------------------------------------- /src/position_tracking/stereo_tracker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "position_tracking/base_tracker.h" 3 | #include "framepoint_generation/stereo_framepoint_generator.h" 4 | 5 | namespace proslam { 6 | 7 | //ds this class processes two subsequent Frames and establishes Framepoint correspondences (tracks) based on the corresponding images 8 | class StereoTracker: public BaseTracker { 9 | 10 | //ds object management 11 | PROSLAM_MAKE_PROCESSING_CLASS(StereoTracker) 12 | 13 | //ds functionality 14 | public: 15 | 16 | //ds magic 17 | virtual void compute(); 18 | 19 | //ds setters/getters 20 | public: 21 | 22 | void setCameraRight(const Camera* camera_right_) {_camera_right = camera_right_;} 23 | void setIntensityImageRight(const cv::Mat& intensity_image_right_) {_intensity_image_right = intensity_image_right_;} 24 | 25 | //ds helpers 26 | protected: 27 | 28 | //ds creates a frame, which is filled by calling the framepoint generator 29 | virtual Frame* _createFrame(); 30 | 31 | //ds attempts to recover framepoints in the current image using the more precise pose estimate, retrieved after pose optimization 32 | virtual void _recoverPoints(Frame* current_frame_); 33 | 34 | //ds attributes 35 | protected: 36 | 37 | //ds configuration 38 | const Camera* _camera_right = nullptr; 39 | 40 | //ds processing 41 | cv::Mat _intensity_image_right; 42 | 43 | //ds specified generator instance 44 | StereoFramePointGenerator* _stereo_framepoint_generator = nullptr; 45 | }; 46 | } 47 | -------------------------------------------------------------------------------- /src/relocalization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_relocalization_library 2 | relocalizer.cpp 3 | ) 4 | 5 | target_link_libraries(srrg_proslam_relocalization_library 6 | srrg_proslam_aligners_library 7 | ) 8 | -------------------------------------------------------------------------------- /src/relocalization/closure.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "types/local_map.h" 3 | 4 | namespace proslam { 5 | 6 | //ds this class is a container for all correspondence objects between 2 local maps (produced by the relocalization module) 7 | class Closure { 8 | 9 | //ds exported types 10 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | //ds match information between two landmark states 13 | struct Candidate { 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | Candidate(Landmark* landmark_query_, 16 | Landmark* landmark_reference_, 17 | const Count& matching_distance_hamming_): query(landmark_query_), 18 | reference(landmark_reference_), 19 | matching_distance_hamming(matching_distance_hamming_) {} 20 | 21 | Landmark* query; 22 | Landmark* reference; 23 | const Count matching_distance_hamming; 24 | }; 25 | 26 | typedef std::vector> CandidateVector; 27 | typedef std::pair CandidateMapElement; 28 | typedef std::map, Eigen::aligned_allocator> CandidateMap; 29 | 30 | //ds container for a single correspondence pair (produced by the relocalization module) 31 | struct Correspondence { 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | Correspondence(Landmark* landmark_query_, 34 | Landmark* landmark_reference_, 35 | const Count& matching_count_, 36 | const real& matching_ratio_): query(landmark_query_), 37 | reference(landmark_reference_), 38 | matching_count(matching_count_), 39 | matching_ratio(matching_ratio_) {} 40 | 41 | Landmark* query; 42 | Landmark* reference; 43 | const Count matching_count; 44 | const real matching_ratio; 45 | 46 | //ds determined as inlier in registration algorithm (e.g. ICP) 47 | bool is_inlier = false; 48 | }; 49 | 50 | typedef std::vector CorrespondencePointerVector; 51 | 52 | //ds object life 53 | public: 54 | 55 | //ds ctor 56 | Closure(const LocalMap* local_map_query_, 57 | const LocalMap* local_map_reference_, 58 | const Count& absolute_number_of_matches_, 59 | const real& relative_number_of_matches_, 60 | const CorrespondencePointerVector& correspondences_): local_map_query(local_map_query_), 61 | local_map_reference(local_map_reference_), 62 | absolute_number_of_matches(absolute_number_of_matches_), 63 | relative_number_of_matches(relative_number_of_matches_), 64 | correspondences(correspondences_) {} 65 | 66 | //ds dtor 67 | ~Closure() { 68 | 69 | //ds if closure is invalidated it has not been consumed by the world map and is to be freed 70 | if (!is_valid) { 71 | for (const Correspondence* correspondence: correspondences) { 72 | delete correspondence; 73 | } 74 | } 75 | correspondences.clear(); 76 | } 77 | 78 | //ds attributes 79 | public: 80 | 81 | const LocalMap* local_map_query; 82 | const LocalMap* local_map_reference; 83 | const Count absolute_number_of_matches; 84 | const real relative_number_of_matches; 85 | CorrespondencePointerVector correspondences; 86 | 87 | //ds attributes set after registration 88 | TransformMatrix3D query_to_reference = TransformMatrix3D::Identity(); 89 | real icp_inlier_ratio = 0; 90 | Count icp_number_of_iterations = 0; 91 | Count icp_number_of_inliers = 0; 92 | bool is_valid = false; 93 | 94 | }; 95 | 96 | typedef std::vector ClosurePointerVector; 97 | 98 | } 99 | -------------------------------------------------------------------------------- /src/relocalization/relocalizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "aligners/xyz_aligner.h" 3 | #include "closure.h" 4 | 5 | namespace proslam { 6 | 7 | //ds this class computes potential loop closures for a given local map query (the extent of computed detail can be steered easily using the different methods) 8 | class Relocalizer { 9 | 10 | //ds object management 11 | PROSLAM_MAKE_PROCESSING_CLASS(Relocalizer) 12 | 13 | //ds interface 14 | public: 15 | 16 | //ds retrieve loop closure candidates for the given local map, containing descriptors for its landmarks 17 | void detectClosures(LocalMap* local_map_query_); 18 | 19 | //ds geometric verification and determination of spatial relation between closure set 20 | void registerClosures(); 21 | 22 | //ds clear currently available closure buffer 23 | void clear(); 24 | 25 | //! @brief keeps only a single closure, based on the maximum relative number of correspodences TODO add proper constraints 26 | void prune(); 27 | 28 | //ds getters/setters 29 | public: 30 | 31 | inline const ClosurePointerVector& closures() const {return _closures;} 32 | XYZAlignerPtr aligner() {return _aligner;} 33 | 34 | //ds helpers 35 | protected: 36 | 37 | //ds retrieve correspondences from matches 38 | inline Closure::Correspondence* _getCorrespondenceNN(const Closure::CandidateVector& matches_); 39 | 40 | protected: 41 | 42 | //ds buffer of found closures (last compute call) 43 | ClosurePointerVector _closures; 44 | 45 | //ds local map to local map alignment 46 | XYZAlignerPtr _aligner = nullptr; 47 | 48 | //ds database of visited places (= local maps), storing a descriptor vector for each place 49 | HBSTTree _place_database; 50 | 51 | //ds local maps that have been added to the place database (in order of calls) 52 | ConstLocalMapPointerVector _added_local_maps; 53 | 54 | //ds correspondence retrieval buffer 55 | std::set _mask_id_references_for_correspondences; 56 | 57 | private: 58 | 59 | CREATE_CHRONOMETER(overall) 60 | 61 | }; 62 | } 63 | -------------------------------------------------------------------------------- /src/system/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ds export assembly as library 2 | add_library(srrg_proslam_slam_assembly_library slam_assembly.cpp) 3 | target_link_libraries(srrg_proslam_slam_assembly_library 4 | srrg_proslam_map_optimization_library 5 | srrg_proslam_position_tracking_library 6 | srrg_proslam_relocalization_library 7 | srrg_proslam_visualization_library 8 | srrg_messages_library 9 | srrg_system_utils_library 10 | ) 11 | -------------------------------------------------------------------------------- /src/system/slam_assembly.h: -------------------------------------------------------------------------------- 1 | #include "qapplication.h" 2 | #include 3 | #include 4 | 5 | #include "srrg_messages/message_reader.h" 6 | #include "srrg_messages/message_timestamp_synchronizer.h" 7 | 8 | #include "position_tracking/base_tracker.h" 9 | #include "map_optimization/graph_optimizer.h" 10 | #include "relocalization/relocalizer.h" 11 | #include "visualization/image_viewer.h" 12 | #include "visualization/map_viewer.h" 13 | #include "framepoint_generation/stereo_framepoint_generator.h" 14 | 15 | namespace proslam { 16 | 17 | //ds simple assembly of the different SLAM modules provided by ProSLAM 18 | class SLAMAssembly { 19 | 20 | //ds object management 21 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | //ds default constructor - already allocating required default objects 24 | SLAMAssembly(ParameterCollection* parameters_); 25 | 26 | //ds default destructor 27 | ~SLAMAssembly(); 28 | 29 | //ds functionality 30 | public: 31 | 32 | //ds attempts to load the camera configuration based on the current input setting 33 | void loadCamerasFromMessageFile(); 34 | 35 | //ds attempts to load the camera configuration based on the current input setting 36 | void loadCameras(Camera* camera_left_, Camera* camera_right_); 37 | 38 | //ds initializes gui components 39 | void initializeGUI(std::shared_ptr ui_server_); 40 | 41 | //ds updates GUI components with current state 42 | void updateGUI(); 43 | 44 | //! @brief triggers draw functions of all connected viewers 45 | void draw(); 46 | 47 | //! @brief saves current pose graph to disk 48 | //! @param[in] file_name_ desired file name for the g2o outfile 49 | void writePoseGraphToFile(const std::string& file_name_ = "pose_graph.g2o") const; 50 | 51 | //! @brief playback txt_io message file 52 | void playbackMessageFile(); 53 | 54 | //! @brief thread wrapping 55 | std::shared_ptr playbackMessageFileInThread() { 56 | return std::make_shared([=] {playbackMessageFile();}); 57 | } 58 | 59 | //! @brief process a pair of rectified and undistorted stereo images 60 | void process(const cv::Mat& intensity_image_left_, 61 | const cv::Mat& intensity_image_right_, 62 | const double& timestamp_image_left_seconds_ = 0, 63 | const bool& use_odometry_ = false, 64 | const TransformMatrix3D& odometry_ = TransformMatrix3D::Identity()); 65 | 66 | //ds prints extensive run summary 67 | void printReport() const; 68 | 69 | //! @brief dump trajectory to file (in KITTI benchmark format: 4x4 isometries per line) 70 | //! @param[in] file_name_ text file path in which the poses are saved to 71 | void writeTrajectoryKITTI(const std::string& file_name_ = "") const {if (_world_map) {_world_map->writeTrajectoryKITTI(file_name_);}} 72 | 73 | //! @brief dump trajectory to file (in TUM benchmark format: timestamp x z y and qx qy qz qw per line) 74 | //! @param[in] filename_ text file in which the poses are saved to 75 | void writeTrajectoryTUM(const std::string& file_name_ = "") const {if (_world_map) {_world_map->writeTrajectoryTUM(file_name_);}} 76 | 77 | //! @brief save trajectory to a vector 78 | //! @param[in,out] poses_ vector with poses, set in the function 79 | template 80 | void writeTrajectory(std::vector, Eigen::aligned_allocator>>& poses_) const {if (_world_map) {_world_map->writeTrajectory(poses_);}} 81 | 82 | //! @brief save trajectory to a vector with timestamps 83 | //! @param[in,out] poses_ vector with timestamps and poses, set in the function 84 | template 85 | void writeTrajectoryWithTimestamps(std::vector>>& poses_) const {if (_world_map) {_world_map->writeTrajectoryWithTimestamps(poses_);}} 86 | 87 | //! @brief resets the complete pipeline, releasing memory 88 | void reset(); 89 | 90 | //ds getters/setters 91 | public: 92 | 93 | void requestTermination() {_is_termination_requested = true;} 94 | const bool isViewerOpen() const {return _is_viewer_open;} 95 | const double currentFPS() const {return _current_fps;} 96 | const double averageNumberOfLandmarksPerFrame() const {return _tracker->totalNumberOfLandmarks()/_number_of_processed_frames;} 97 | const double averageNumberOfTracksPerFrame() const {return _tracker->totalNumberOfTrackedPoints()/_number_of_processed_frames;} 98 | const Count numberOfRecursiveRegistrations() const {return _tracker->numberOfRecursiveRegistrations();} 99 | const real meanTrackingRatio() const {return _tracker->meanTrackingRatio();} 100 | const real meanTriangulationRatio() const {return dynamic_cast(_tracker->framepointGenerator())->meanTriangulationSuccessRatio();} 101 | 102 | //ds helpers: 103 | protected: 104 | 105 | void _createStereoTracker(Camera* camera_left_, Camera* camera_right_); 106 | 107 | void _createDepthTracker(Camera* camera_left_, Camera* camera_right_); 108 | 109 | //ds SLAM modules 110 | protected: 111 | 112 | //! @brief all configurable system parameters 113 | ParameterCollection* _parameters; 114 | 115 | //ds the SLAM map, containing landmarks and trajectory 116 | WorldMap* _world_map; 117 | 118 | //ds pose graph optimization handler 119 | GraphOptimizer* _graph_optimizer; 120 | 121 | //ds relocalization module 122 | Relocalizer* _relocalizer; 123 | 124 | //ds tracking component, deriving the robots odometry 125 | BaseTracker* _tracker; 126 | 127 | //ds loaded sensors 128 | Camera* _camera_left; 129 | Camera* _camera_right; 130 | 131 | //ds visualization only 132 | protected: 133 | 134 | //ds Qt UI server 135 | std::shared_ptr _ui_server; 136 | 137 | //ds raw image input processing viewer 138 | std::shared_ptr _image_viewer; 139 | 140 | //ds 3D output viewers 141 | std::shared_ptr _map_viewer; 142 | std::shared_ptr _minimap_viewer; 143 | 144 | // std::atomic _new_image_available; 145 | 146 | //ds playback components 147 | protected: 148 | 149 | //! @brief srrg message parser 150 | srrg_core::MessageReader _message_reader; 151 | 152 | //ds txt_io message synchronizer 153 | srrg_core::MessageTimestampSynchronizer _synchronizer; 154 | 155 | //! @brief termination check - terminates processing loop 156 | std::atomic _is_termination_requested; 157 | 158 | //! @brief flag that is checked if an OpenGL or OpenCV window is currently active 159 | std::atomic _is_viewer_open; 160 | 161 | //ds informative only 162 | protected: 163 | 164 | //! @brief total system processing runtime 165 | double _processing_time_total_seconds = 0; 166 | 167 | //! @brief frame-wise processing times 168 | std::vector _processing_times_seconds; 169 | 170 | //! @brief total number of processed frames 171 | Count _number_of_processed_frames = 0; 172 | 173 | //! @brief current average fps 174 | double _current_fps = 0; 175 | }; 176 | } 177 | -------------------------------------------------------------------------------- /src/types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_types_library 2 | parameters.cpp 3 | frame.cpp 4 | local_map.cpp 5 | world_map.cpp 6 | frame_point.cpp 7 | landmark.cpp 8 | camera.cpp 9 | ) 10 | 11 | target_link_libraries(srrg_proslam_types_library 12 | srrg_system_utils_library 13 | ${OpenCV_LIBS} 14 | yaml-cpp 15 | ) 16 | -------------------------------------------------------------------------------- /src/types/camera.cpp: -------------------------------------------------------------------------------- 1 | #include "camera.h" 2 | 3 | namespace proslam { 4 | 5 | Count Camera::_instances = 0; 6 | 7 | Camera::Camera(const Count& image_rows_, 8 | const Count& image_cols_, 9 | const CameraMatrix& camera_matrix_, 10 | const TransformMatrix3D& camera_to_robot_): _identifier(_instances), 11 | _number_of_image_rows(image_rows_), 12 | _number_of_image_cols(image_cols_) { 13 | ++_instances; 14 | setCameraMatrix(camera_matrix_); 15 | setCameraToRobot(camera_to_robot_); 16 | LOG_INFO(std::cerr << "Camera::Camera|constructed" << std::endl) 17 | } 18 | 19 | const bool Camera::isInFieldOfView(const ImageCoordinates& image_coordinates_) const { 20 | assert(image_coordinates_.z() == 1); 21 | return ((image_coordinates_.x() >= 0 && image_coordinates_.x() <= _number_of_image_cols)&& 22 | (image_coordinates_.y() >= 0 && image_coordinates_.y() <= _number_of_image_rows)); 23 | } 24 | 25 | void Camera::setCameraMatrix(const CameraMatrix& camera_matrix_) { 26 | _camera_matrix = camera_matrix_; 27 | _inverse_camera_matrix = _camera_matrix.inverse(); 28 | } 29 | 30 | void Camera::setCameraToRobot(const TransformMatrix3D& camera_to_robot_) { 31 | _camera_to_robot = camera_to_robot_; 32 | _robot_to_camera = _camera_to_robot.inverse(); 33 | } 34 | 35 | void Camera::writeConfiguration(std::ostream& stream_) const { 36 | stream_ << "ID: " << _identifier << " resolution: [rows = " << _number_of_image_rows << "] x [cols = " << _number_of_image_cols << "]" << std::endl; 37 | stream_ << "camera calibration matrix: K = \n" << _camera_matrix << std::endl; 38 | stream_ << "camera projection matrix: P = \n" << _projection_matrix << std::endl; 39 | stream_ << "distortion coefficients: D = \n" << _distortion_coefficients.transpose() << std::endl; 40 | stream_ << "rectification matrix: R = \n" << _rectification_matrix << std::endl; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/types/camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "definitions.h" 3 | 4 | namespace proslam { 5 | 6 | //! @class single pinhole model based camera object 7 | class Camera { 8 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | 10 | //ds object handling 11 | public: 12 | 13 | //! @constructor a camera can be constructed based on image dimensions and a camera matrix 14 | //! @param[in] number_of_image_rows_ number of rows of the image (pixel height) 15 | //! @param[in] number_of_image_cols_ number of columns of the image (pixel width) 16 | //! @param[in] camera_matrix_ pinhole model camera matrix 17 | //! @param[in] camera_to_robot_ camera to robot transform (usually constant during operation) 18 | Camera(const Count& number_of_image_rows_, 19 | const Count& number_of_image_cols_, 20 | const CameraMatrix& camera_matrix_, 21 | const TransformMatrix3D& camera_to_robot_ = TransformMatrix3D::Identity()); 22 | 23 | //! @brief prohibit default construction 24 | Camera() = delete; 25 | 26 | //! @brief default destructor 27 | ~Camera() {}; 28 | 29 | //ds getters/setters 30 | public: 31 | 32 | inline const Identifier& identifier() const {return _identifier;} 33 | inline const Count& numberOfImageRows() const {return _number_of_image_rows;} 34 | inline const Count& numberOfImageCols() const {return _number_of_image_cols;} 35 | 36 | //! @brief check whether an image point is contained in the current image dimensions or not 37 | //! @param[in] image_coordinates_ 2D pixel coordinates of the point to evaluate 38 | //! @return true: if image_coordinates_ lies in the image plane, false: otherwise 39 | const bool isInFieldOfView(const ImageCoordinates& image_coordinates_) const; 40 | 41 | void setNumberOfImageRows(const Count& number_of_image_rows_) {_number_of_image_rows = number_of_image_rows_;} 42 | void setNumberOfImageCols(const Count& number_of_image_cols_) {_number_of_image_cols = number_of_image_cols_;} 43 | inline const CameraMatrix& cameraMatrix() const {return _camera_matrix;} 44 | void setCameraMatrix(const CameraMatrix& camera_matrix_); 45 | inline const ProjectionMatrix& projectionMatrix() const {return _projection_matrix;} 46 | void setProjectionMatrix(const ProjectionMatrix& projection_matrix_) {_projection_matrix = projection_matrix_;} 47 | inline const Vector3& baselineHomogeneous() const {return _baseline_homogeneous;} 48 | void setBaselineHomogeneous(const Vector3& baseline_homogeneous_) {_baseline_homogeneous = baseline_homogeneous_;} 49 | inline const TransformMatrix3D& cameraToRobot() const {return _camera_to_robot;} 50 | inline const TransformMatrix3D& robotToCamera() const {return _robot_to_camera;} 51 | void setCameraToRobot(const TransformMatrix3D& camera_to_robot_); 52 | inline const Matrix3& rectificationMatrix() const {return _rectification_matrix;} 53 | void setRectificationMatrix(const Matrix3& rectification_matrix_) {_rectification_matrix = rectification_matrix_;} 54 | inline const Vector5& distortionCoefficients() const {return _distortion_coefficients;} 55 | void setDistortionCoefficients(const Vector5& distortion_coefficients_) {_distortion_coefficients = distortion_coefficients_;} 56 | 57 | //! @brief write object configuration to stream 58 | void writeConfiguration(std::ostream& stream_) const; 59 | 60 | //ds attributes 61 | protected: 62 | 63 | //! @brief numerical identifier 64 | Identifier _identifier; 65 | 66 | //! @brief number of rows of the image (pixel height) 67 | Count _number_of_image_rows; 68 | 69 | //! @brief number of columns of the image (pixel width) 70 | Count _number_of_image_cols; 71 | 72 | //! @brief pinhole model camera matrix 73 | CameraMatrix _camera_matrix; 74 | 75 | //! @brief inverse of the pinhole model camera matrix 76 | CameraMatrix _inverse_camera_matrix; 77 | 78 | //! @brief stereo projection matrix (must be set manually with setProjectionMatrix) TODO be removed, instead use baseline 79 | ProjectionMatrix _projection_matrix = ProjectionMatrix::Zero(); 80 | 81 | //! @brief stereo camera basline in homogeneous coordinates 82 | Vector3 _baseline_homogeneous = Vector3::Zero(); 83 | 84 | //! @brief rectification matrix 85 | Matrix3 _rectification_matrix = Matrix3::Zero(); 86 | 87 | //! @brief distortion coefficients 88 | Vector5 _distortion_coefficients = Vector5::Zero(); 89 | 90 | //! @brief camera to robot transform (usually constant during operation) 91 | TransformMatrix3D _camera_to_robot; 92 | 93 | //! @brief robot to camera transform (usually constant during operation) 94 | TransformMatrix3D _robot_to_camera; 95 | 96 | //ds class specific 97 | private: 98 | 99 | //! @brief object instance count (used for identifier generation) 100 | static Count _instances; 101 | }; 102 | } 103 | -------------------------------------------------------------------------------- /src/types/definitions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #if CV_MAJOR_VERSION == 2 12 | //ds no specifics 13 | #elif CV_MAJOR_VERSION == 3 14 | #include 15 | #else 16 | #error OpenCV version not supported 17 | #endif 18 | 19 | #include "srrg_system_utils/system_utils.h" 20 | #include "srrg_types/types.hpp" 21 | 22 | namespace proslam { 23 | 24 | //ds pattern helper macros 25 | #define PROSLAM_CONCATENATE(A, B) A ## B 26 | 27 | //ds pattern macros 28 | #define PROSLAM_MAKE_PROCESSING_CLASS(CLASS_NAME) \ 29 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ 30 | CLASS_NAME(PROSLAM_CONCATENATE(CLASS_NAME, Parameters)* parameters_); \ 31 | virtual void configure(); \ 32 | virtual ~CLASS_NAME(); \ 33 | private: PROSLAM_CONCATENATE(CLASS_NAME, Parameters)* _parameters; \ 34 | public: PROSLAM_CONCATENATE(CLASS_NAME, Parameters)* parameters() {return _parameters;} 35 | 36 | #define PROSLAM_MAKE_PROCESSING_SUBCLASS(SUBCLASS_NAME, PARAMETERS_TYPE) \ 37 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ 38 | SUBCLASS_NAME(PARAMETERS_TYPE* parameters_); \ 39 | virtual ~SUBCLASS_NAME(); 40 | 41 | //ds descriptor bit width 42 | #ifndef SRRG_PROSLAM_DESCRIPTOR_SIZE_BITS 43 | #define SRRG_PROSLAM_DESCRIPTOR_SIZE_BITS 256 44 | #endif 45 | #define DESCRIPTOR_SIZE_BYTES SRRG_PROSLAM_DESCRIPTOR_SIZE_BITS/8 46 | #define SRRG_PROSLAM_DESCRIPTOR_NORM cv::NORM_HAMMING 47 | 48 | //ds adjust floating point precision 49 | typedef double real; 50 | 51 | //ds existential types 52 | typedef Eigen::Matrix PointCoordinates; 53 | typedef std::vector> PointCoordinatesVector; 54 | typedef Eigen::Matrix ImageCoordinates; 55 | typedef std::vector> ImageCoordinatesVector; 56 | typedef Eigen::Matrix PointColorRGB; 57 | typedef Eigen::Matrix CameraMatrix; 58 | typedef Eigen::Matrix ProjectionMatrix; 59 | typedef Eigen::Transform TransformMatrix3D; 60 | typedef Eigen::Matrix TransformVector3D; 61 | typedef Eigen::Quaternion Quaternion; 62 | typedef uint64_t Identifier; 63 | typedef uint64_t Index; 64 | typedef uint64_t Count; 65 | typedef cv::Mat IntensityImage; 66 | typedef std::vector IntensityImageVector; 67 | typedef cv::Mat DepthImage; 68 | typedef std::vector DepthImageVector; 69 | typedef std::pair PointDrawable; 70 | 71 | //ds generic types 72 | typedef Eigen::Matrix Vector2; 73 | typedef Eigen::Matrix Matrix2; 74 | typedef Eigen::Matrix Vector3; 75 | typedef Eigen::Matrix Vector4; 76 | typedef Eigen::Matrix Matrix3; 77 | typedef Eigen::Matrix Matrix4; 78 | typedef Eigen::Matrix Matrix6; 79 | typedef Eigen::Matrix Vector5; 80 | typedef Eigen::Matrix Vector6; 81 | typedef Eigen::Matrix Matrix1_6; 82 | typedef Eigen::Matrix Matrix3_6; 83 | typedef Eigen::Matrix Matrix4_6; 84 | typedef Eigen::Matrix Matrix2_6; 85 | typedef Eigen::Matrix Matrix1_3; 86 | typedef Eigen::Matrix Matrix2_3; 87 | typedef Eigen::Matrix Matrix6_3; 88 | typedef Eigen::Matrix Matrix6_4; 89 | typedef Eigen::Matrix Matrix3_4; 90 | typedef Eigen::Matrix Matrix4_3; 91 | 92 | //ds cv colors 93 | #define CV_COLOR_CODE_RANDOM cv::Scalar(rand()%255, rand()%255, rand()%255) 94 | #define CV_COLOR_CODE_GREEN cv::Scalar(0, 200, 0) 95 | #define CV_COLOR_CODE_BLUE cv::Scalar(255, 0, 0) 96 | #define CV_COLOR_CODE_DARKBLUE cv::Scalar(150, 0, 0) 97 | #define CV_COLOR_CODE_RED cv::Scalar(0, 0, 255) 98 | #define CV_COLOR_CODE_YELLOW cv::Scalar(0, 255, 255) 99 | #define CV_COLOR_CODE_WHITE cv::Scalar(255, 255, 255) 100 | #define CV_COLOR_CODE_BLACK cv::Scalar(0, 0, 0) 101 | #define CV_COLOR_CODE_ORANGE cv::Scalar(0, 150, 255) 102 | #define CV_COLOR_CODE_DARKGREEN cv::Scalar(0, 150, 0) 103 | #define CV_COLOR_CODE_BROWN cv::Scalar(0, 100, 175) 104 | #define CV_COLOR_CODE_VIOLETT cv::Scalar(255, 0, 255) 105 | #define CV_COLOR_CODE_DARKVIOLETT cv::Scalar(150, 0, 150) 106 | 107 | //ds log levels 108 | enum LoggingLevel {Debug = 0, 109 | Info = 1, 110 | Warning = 2, 111 | Error = 3}; 112 | 113 | //ds timing 114 | #define CREATE_CHRONOMETER(NAME) \ 115 | protected: double _time_consumption_seconds_##NAME = 0; \ 116 | public: const double getTimeConsumptionSeconds_##NAME() const {return _time_consumption_seconds_##NAME;} 117 | #define CHRONOMETER_START(NAME) const double time_start_seconds_##NAME = srrg_core::getTime(); 118 | #define CHRONOMETER_STOP(NAME) _time_consumption_seconds_##NAME += srrg_core::getTime()-time_start_seconds_##NAME; 119 | 120 | //ds print functions 121 | #define BAR "---------------------------------------------------------------------------------------------------------------------------------" 122 | #define DOUBLE_BAR "=================================================================================================================================" 123 | 124 | //ds assertion handling 125 | #define ASSERT(ASSERTION, INFO) \ 126 | assert(ASSERTION && INFO); 127 | 128 | //ds log level 129 | #ifndef SRRG_PROSLAM_LOG_LEVEL 130 | #define SRRG_PROSLAM_LOG_LEVEL 2 131 | #endif 132 | 133 | //ds generic logging macro - called by each implemented logging function 134 | #define LOG_GENERIC(LOGGING_LEVEL, LOGGING_LEVEL_DESCRIPTION, EXPRESSION) \ 135 | std::cerr << srrg_core::getTimestamp() << "|" << LOGGING_LEVEL_DESCRIPTION << "|"; \ 136 | EXPRESSION; 137 | 138 | //ds log levels (defined at compile time) 139 | #if SRRG_PROSLAM_LOG_LEVEL > 2 140 | #define LOG_DEBUG(EXPRESSION) \ 141 | LOG_GENERIC(LoggingLevel::Debug, "DEBUG ", EXPRESSION) 142 | #else 143 | #define LOG_DEBUG(EXPRESSION) 144 | #endif 145 | 146 | #if SRRG_PROSLAM_LOG_LEVEL > 1 147 | #define LOG_INFO(EXPRESSION) \ 148 | LOG_GENERIC(LoggingLevel::Info, "INFO ", EXPRESSION) 149 | #else 150 | #define LOG_INFO(EXPRESSION) 151 | #endif 152 | 153 | #if SRRG_PROSLAM_LOG_LEVEL > 0 154 | #define LOG_WARNING(EXPRESSION) \ 155 | LOG_GENERIC(LoggingLevel::Warning, "WARNING", EXPRESSION) 156 | #else 157 | #define LOG_WARNING(EXPRESSION) 158 | #endif 159 | 160 | #define LOG_ERROR(EXPRESSION) \ 161 | LOG_GENERIC(LoggingLevel::Error, "ERROR ", EXPRESSION) 162 | 163 | } 164 | -------------------------------------------------------------------------------- /src/types/frame.cpp: -------------------------------------------------------------------------------- 1 | #include "frame.h" 2 | 3 | #include "world_map.h" 4 | 5 | namespace proslam { 6 | 7 | Count Frame::_instances = 0; 8 | 9 | Frame::Frame(const WorldMap* context_, 10 | Frame* previous_, 11 | Frame* next_, 12 | const TransformMatrix3D& robot_to_world_, 13 | const double& timestamp_image_left_seconds_): _identifier(_instances), 14 | _timestamp_image_left_seconds(timestamp_image_left_seconds_), 15 | _previous(previous_), 16 | _next(next_), 17 | _local_map(nullptr) { 18 | ++_instances; 19 | if (context_) { 20 | _root = context_->rootFrame(); 21 | } else { 22 | _root = this; 23 | } 24 | setRobotToWorld(robot_to_world_); 25 | _created_points.clear(); 26 | _active_points.clear(); 27 | _keypoints_left.clear(); 28 | _keypoints_right.clear(); 29 | } 30 | 31 | Frame::~Frame() { 32 | clear(); 33 | _keypoints_left.clear(); 34 | _keypoints_right.clear(); 35 | } 36 | 37 | void Frame::setRobotToWorld(const TransformMatrix3D& robot_to_world_, const bool update_local_map_) { 38 | _robot_to_world = robot_to_world_; 39 | _world_to_robot = _robot_to_world.inverse(); 40 | if (_camera_left) { 41 | _camera_left_to_world = robot_to_world_*_camera_left->cameraToRobot(); 42 | _world_to_camera_left = _camera_left_to_world.inverse(); 43 | } 44 | 45 | //ds if the frame is a keyframe 46 | if (_is_keyframe && update_local_map_) { 47 | assert(_local_map); 48 | 49 | //ds update the local map position 50 | _local_map->setLocalMapToWorld(_robot_to_world); 51 | } 52 | 53 | //ds update framepoint world coordinates 54 | updateActivePoints(); 55 | } 56 | 57 | FramePoint* Frame::createFramepoint(const cv::KeyPoint& keypoint_left_, 58 | const cv::Mat& descriptor_left_, 59 | const cv::KeyPoint& keypoint_right_, 60 | const cv::Mat& descriptor_right_, 61 | const PointCoordinates& camera_coordinates_left_, 62 | FramePoint* previous_point_) { 63 | assert(_camera_left != 0); 64 | 65 | //ds allocate a new point connected to the previous one 66 | FramePoint* frame_point = new FramePoint(keypoint_left_, 67 | descriptor_left_, 68 | keypoint_right_, 69 | descriptor_right_, 70 | this); 71 | frame_point->setCameraCoordinatesLeft(camera_coordinates_left_); 72 | frame_point->setRobotCoordinates(_camera_left->cameraToRobot()*camera_coordinates_left_); 73 | frame_point->setWorldCoordinates(this->robotToWorld()*frame_point->robotCoordinates()); 74 | 75 | //ds if there is a previous point 76 | if (previous_point_) { 77 | 78 | //ds connect the framepoints 79 | frame_point->setPrevious(previous_point_); 80 | } else { 81 | 82 | //ds this point has no predecessor 83 | frame_point->setOrigin(frame_point); 84 | } 85 | 86 | //ds update depth statistics for this frame 87 | const real depth_meters = camera_coordinates_left_.z(); 88 | 89 | //ds update depth based on quality 90 | frame_point->setDepthMeters(depth_meters); 91 | 92 | //ds bookkeep each generated point for resize immune memory management (TODO remove costly bookkeeping) 93 | _created_points.push_back(frame_point); 94 | return frame_point; 95 | } 96 | 97 | FramePoint* Frame::createFramepoint(const IntensityFeature* feature_left_, 98 | const IntensityFeature* feature_right_, 99 | const PointCoordinates& camera_coordinates_left_, 100 | FramePoint* previous_point_) { 101 | assert(_camera_left != 0); 102 | 103 | //ds allocate a new point connected to the previous one 104 | FramePoint* frame_point = new FramePoint(feature_left_, feature_right_, this); 105 | frame_point->setCameraCoordinatesLeft(camera_coordinates_left_); 106 | frame_point->setRobotCoordinates(_camera_left->cameraToRobot()*camera_coordinates_left_); 107 | frame_point->setWorldCoordinates(this->robotToWorld()*frame_point->robotCoordinates()); 108 | 109 | //ds if there is a previous point 110 | if (previous_point_) { 111 | 112 | //ds connect the framepoints 113 | frame_point->setPrevious(previous_point_); 114 | } else { 115 | 116 | //ds this point has no predecessor 117 | frame_point->setOrigin(frame_point); 118 | } 119 | 120 | //ds update depth statistics for this frame 121 | const real depth_meters = camera_coordinates_left_.z(); 122 | 123 | //ds update depth based on quality 124 | frame_point->setDepthMeters(depth_meters); 125 | 126 | //ds bookkeep each generated point for resize immune memory management (TODO remove costly bookkeeping) 127 | _created_points.push_back(frame_point); 128 | return frame_point; 129 | } 130 | 131 | void Frame::clear() { 132 | for (const FramePoint* frame_point: _created_points) { 133 | delete frame_point; 134 | } 135 | _created_points.clear(); 136 | _active_points.clear(); 137 | } 138 | 139 | void Frame::updateActivePoints() { 140 | for (FramePoint* point: _active_points) { 141 | point->setWorldCoordinates(_robot_to_world*point->robotCoordinates()); 142 | } 143 | } 144 | } 145 | -------------------------------------------------------------------------------- /src/types/frame.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "parameters.h" 3 | #include "camera.h" 4 | #include "frame_point.h" 5 | 6 | namespace proslam { 7 | 8 | //ds forward declarations 9 | class LocalMap; 10 | class WorldMap; 11 | 12 | //ds this class encapsulates all data gained from the processing of a stereo image pair 13 | class Frame { 14 | 15 | //ds exported types 16 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | //ds defines one of the two tracker states 19 | enum Status {Localizing, Tracking}; 20 | 21 | //ds prohibit default construction 22 | Frame() = delete; 23 | 24 | //ds object handling 25 | public: //ds TODO protect for factory 26 | 27 | //ds frame construction in the WorldMap context 28 | Frame(const WorldMap* context_, 29 | Frame* previous_, 30 | Frame* next_, 31 | const TransformMatrix3D& robot_to_world_, 32 | const double& timestamp_image_left_seconds_); 33 | 34 | //ds FramePoints cleanup 35 | ~Frame(); 36 | 37 | //ds getters/setters 38 | public: 39 | 40 | const Identifier& identifier() const {return _identifier;} 41 | 42 | inline void setTimestampImageLeftSeconds(const double& timestamp_image_left_seconds_) {_timestamp_image_left_seconds = timestamp_image_left_seconds_;} 43 | const double& timestampImageLeftSeconds() const {return _timestamp_image_left_seconds;} 44 | 45 | inline const Frame* root() const {return _root;} 46 | inline void setRoot(const Frame* root_) {_root = root_;} 47 | 48 | inline Frame* previous() {return _previous;} 49 | inline const Frame* previous() const {return _previous;} 50 | void setPrevious(Frame* previous_) {_previous = previous_;} 51 | 52 | inline Frame* next() {return _next;} 53 | inline const Frame* next() const {return _next;} 54 | void setNext(Frame* next_) {_next = next_;} 55 | 56 | void breakTrack() {_is_track_broken = true;} 57 | const bool isTrackBroken() const {return _is_track_broken;} 58 | 59 | void setProjectionTrackingDistancePixels(const uint32_t& projection_tracking_distance_pixels_) {_projection_tracking_distance_pixels = projection_tracking_distance_pixels_;} 60 | const uint32_t& projectionTrackingDistancePixels() const {return _projection_tracking_distance_pixels;} 61 | 62 | inline std::vector& keypointsLeft() {return _keypoints_left;} 63 | inline std::vector& keypointsRight() {return _keypoints_right;} 64 | inline cv::Mat& descriptorsLeft() {return _descriptors_left;} 65 | inline cv::Mat& descriptorsRight() {return _descriptors_right;} 66 | 67 | inline const Camera* cameraLeft() const {return _camera_left;} 68 | void setCameraLeft(const Camera* camera_) {_camera_left = camera_;} 69 | 70 | inline const Camera* cameraRight() const {return _camera_right;} 71 | void setCameraRight(const Camera* camera_) {_camera_right = camera_;} 72 | 73 | inline const TransformMatrix3D& robotToWorld() const {return _robot_to_world;} 74 | void setRobotToWorld(const TransformMatrix3D& robot_to_world_, const bool update_local_map_ = false); 75 | inline const TransformMatrix3D& worldToRobot() const {return _world_to_robot;} 76 | inline const TransformMatrix3D& cameraLeftToWorld() const {return _camera_left_to_world;} 77 | inline const TransformMatrix3D& worldToCameraLeft() const {return _world_to_camera_left;} 78 | 79 | inline const TransformMatrix3D& frameToLocalMap() const {return _frame_to_local_map;} 80 | inline const TransformMatrix3D& localMapToFrame() const {return _local_map_to_frame;} 81 | 82 | //ds visualization only 83 | void setRobotToWorldGroundTruth(const TransformMatrix3D& robot_to_world_ground_truth_) {_robot_to_world_ground_truth = robot_to_world_ground_truth_;} 84 | const TransformMatrix3D& robotToWorldGroundTruth() const {return _robot_to_world_ground_truth;} 85 | 86 | inline const FramePointPointerVector& points() const {return _active_points;} 87 | inline FramePointPointerVector& points() {return _active_points;} 88 | 89 | //ds request a new framepoint instance with an optional link to a previous point (track) 90 | FramePoint* createFramepoint(const cv::KeyPoint& keypoint_left_, 91 | const cv::Mat& descriptor_left_, 92 | const cv::KeyPoint& keypoint_right_, 93 | const cv::Mat& descriptor_right_, 94 | const PointCoordinates& camera_coordinates_left_, 95 | FramePoint* previous_point_ = 0); 96 | 97 | //ds request a new framepoint instance with an optional link to a previous point (track) 98 | FramePoint* createFramepoint(const IntensityFeature* feature_left_, 99 | const IntensityFeature* feature_right_, 100 | const PointCoordinates& camera_coordinates_left_, 101 | FramePoint* previous_point_ = 0); 102 | 103 | //! @brief created framepoints by this factory 104 | inline const FramePointPointerVector& createdPoints() const {return _created_points;} 105 | 106 | inline const cv::Mat& intensityImageLeft() const {return _intensity_image_left;} 107 | void setIntensityImageLeft(const cv::Mat intensity_image_) {_intensity_image_left = intensity_image_;} 108 | 109 | inline const cv::Mat& intensityImageRight() const {return _intensity_image_right;} 110 | void setIntensityImageRight(const cv::Mat intensity_image_) {_intensity_image_right = intensity_image_;} 111 | 112 | inline const Status& status() const {return _status;} 113 | void setStatus(const Status& status_) {_status = status_;} 114 | 115 | void setLocalMap(LocalMap* local_map_) {_local_map = local_map_;} 116 | inline LocalMap* localMap() {return _local_map;} 117 | inline const LocalMap* localMap() const {return _local_map;} 118 | void setFrameToLocalMap(const TransformMatrix3D& frame_to_local_map_) {_frame_to_local_map = frame_to_local_map_; _local_map_to_frame = _frame_to_local_map.inverse();} 119 | void setIsKeyframe(const bool& is_keyframe_) {_is_keyframe = is_keyframe_;} 120 | inline const bool isKeyframe() const {return _is_keyframe;} 121 | 122 | //ds free all point instances 123 | void clear(); 124 | 125 | //ds update framepoint world coordinates 126 | void updateActivePoints(); 127 | 128 | Count _number_of_detected_keypoints = 0; 129 | 130 | //ds reset allocated object counter 131 | static void reset() {_instances = 0;} 132 | 133 | //ds attributes 134 | protected: 135 | 136 | //ds unique identifier for a landmark (exists once in memory) 137 | const Identifier _identifier; 138 | 139 | //! @brief full acquisition timestamp of _intensity_image_left in seconds 140 | double _timestamp_image_left_seconds; 141 | 142 | //ds tracker status at the time of creation of this instance 143 | Status _status = Localizing; 144 | 145 | //ds links to preceding and subsequent instances 146 | Frame* _previous = 0; 147 | Frame* _next = 0; 148 | 149 | //! @brief flag, set if track broke during processing this 150 | bool _is_track_broken = false; 151 | 152 | //! @brief pixel tracking distance used for this frame 153 | uint32_t _projection_tracking_distance_pixels = 0; 154 | 155 | //! @brief detected keypoints at the time of creation of the Frame 156 | std::vector _keypoints_left; 157 | std::vector _keypoints_right; 158 | 159 | //! @brief extracted descriptors associated to the keypoints at the time of creation of the Frame 160 | cv::Mat _descriptors_left; 161 | cv::Mat _descriptors_right; 162 | 163 | //! @brief bookkeeping: all created framepoints for this frame (create function) 164 | FramePointPointerVector _created_points; 165 | 166 | //! @brief bookkeeping: active (used) framepoints in the pipeline (a subset of _created_points) 167 | FramePointPointerVector _active_points; 168 | 169 | //ds spatials 170 | TransformMatrix3D _frame_to_local_map = TransformMatrix3D::Identity(); 171 | TransformMatrix3D _local_map_to_frame = TransformMatrix3D::Identity(); 172 | TransformMatrix3D _robot_to_world = TransformMatrix3D::Identity(); 173 | TransformMatrix3D _world_to_robot = TransformMatrix3D::Identity(); 174 | 175 | TransformMatrix3D _camera_left_to_world = TransformMatrix3D::Identity(); 176 | TransformMatrix3D _world_to_camera_left = TransformMatrix3D::Identity(); 177 | 178 | //ds stereo camera configuration affiliated with this frame 179 | const Camera* _camera_left = 0; 180 | const Camera* _camera_right = 0; 181 | 182 | //ds to support arbitrary number of rgb/depth image combinations 183 | cv::Mat _intensity_image_left; 184 | cv::Mat _intensity_image_right; 185 | 186 | //ds link to a local map if the frame is part of one 187 | LocalMap* _local_map; 188 | bool _is_keyframe = false; 189 | 190 | //ds access 191 | friend class WorldMap; 192 | 193 | //ds visualization only 194 | TransformMatrix3D _robot_to_world_ground_truth = TransformMatrix3D::Identity(); 195 | const Frame* _root; 196 | 197 | //ds class specific 198 | private: 199 | 200 | //ds inner instance count - incremented upon constructor call (also unsuccessful calls) 201 | static Count _instances; 202 | }; 203 | 204 | typedef std::vector FramePointerVector; 205 | typedef std::pair FramePointerMapElement; 206 | typedef std::map FramePointerMap; 207 | } 208 | -------------------------------------------------------------------------------- /src/types/frame_point.cpp: -------------------------------------------------------------------------------- 1 | #include "frame_point.h" 2 | #include "landmark.h" 3 | 4 | namespace proslam { 5 | 6 | Count FramePoint::_instances = 0; 7 | 8 | FramePoint::FramePoint(const cv::KeyPoint& keypoint_left_, 9 | const cv::Mat& descriptor_left_, 10 | const cv::KeyPoint& keypoint_right_, 11 | const cv::Mat& descriptor_right_, 12 | Frame* frame_): row(keypoint_left_.pt.y), 13 | col(keypoint_left_.pt.x), 14 | _identifier(_instances), 15 | _keypoint_left(keypoint_left_), 16 | _keypoint_right(keypoint_right_), 17 | _descriptor_left(descriptor_left_), 18 | _descriptor_right(descriptor_right_), 19 | _disparity_pixels(keypoint_left_.pt.x-keypoint_right_.pt.x), 20 | _image_coordinates_left(PointCoordinates(keypoint_left_.pt.x, keypoint_left_.pt.y, 1)), 21 | _image_coordinates_right(PointCoordinates(keypoint_right_.pt.x, keypoint_right_.pt.y, 1)) { 22 | ++_instances; 23 | setFrame(frame_); 24 | } 25 | 26 | FramePoint::FramePoint(const IntensityFeature* feature_left_, 27 | const IntensityFeature* feature_right_, 28 | Frame* frame_): FramePoint(feature_left_->keypoint, feature_left_->descriptor, feature_right_->keypoint, feature_right_->descriptor, frame_) { 29 | _feature_left = feature_left_; 30 | _feature_right = feature_right_; 31 | } 32 | 33 | FramePoint::~FramePoint() { 34 | delete _feature_left; 35 | delete _feature_right; 36 | } 37 | 38 | void FramePoint::setPrevious(FramePoint* previous_) { 39 | 40 | //ds update linked list 41 | previous_->_next = this; 42 | _previous = previous_; 43 | 44 | //ds update current 45 | setLandmark(previous_->landmark()); 46 | setTrackLength(previous_->trackLength()+1); 47 | setOrigin(previous_->origin()); 48 | } 49 | } //namespace proslam 50 | -------------------------------------------------------------------------------- /src/types/landmark.cpp: -------------------------------------------------------------------------------- 1 | #include "landmark.h" 2 | #include "local_map.h" 3 | 4 | namespace proslam { 5 | 6 | Count Landmark::_instances = 0; 7 | 8 | Landmark::Landmark(FramePoint* origin_, const LandmarkParameters* parameters_): _identifier(_instances), 9 | _origin(origin_), 10 | _parameters(parameters_) { 11 | ++_instances; 12 | _measurements.clear(); 13 | _appearance_map.clear(); 14 | _descriptors.clear(); 15 | _local_maps.clear(); 16 | 17 | //ds compute initial position estimate as rude average of the track 18 | //ds we do not weight the measurements with disparity/inverse depth here 19 | //ds since invalid, small depths can lead to a fatal initial guess 20 | _world_coordinates.setZero(); 21 | FramePoint* framepoint = origin_; 22 | while (framepoint) { 23 | framepoint->setLandmark(this); 24 | _measurements.push_back(Measurement(framepoint)); 25 | _origin = framepoint; 26 | _world_coordinates += framepoint->worldCoordinates(); 27 | framepoint = framepoint->previous(); 28 | } 29 | _world_coordinates /= _measurements.size(); 30 | _number_of_updates = _measurements.size(); 31 | } 32 | 33 | Landmark::~Landmark() { 34 | _appearance_map.clear(); 35 | _measurements.clear(); 36 | _descriptors.clear(); 37 | _local_maps.clear(); 38 | } 39 | 40 | void Landmark::replace(const HBSTMatchable* matchable_old_, HBSTMatchable* matchable_new_) { 41 | 42 | //ds remove the old matchable and check for failure 43 | if (_appearance_map.erase(matchable_old_) != 1) { 44 | LOG_WARNING(std::cerr << "Landmark::replace|" << _identifier << "|unable to erase old HBSTMatchable: " << matchable_old_ << std::endl) 45 | } 46 | 47 | //ds insert new matchable - not critical if already present (same landmark in subsequent local maps) 48 | _appearance_map.insert(std::make_pair(matchable_new_, matchable_new_)); 49 | } 50 | 51 | void Landmark::update(FramePoint* point_) { 52 | _last_update = point_; 53 | 54 | //ds update appearance history (left descriptors only) 55 | _descriptors.push_back(point_->descriptorLeft()); 56 | _measurements.push_back(Measurement(point_)); 57 | 58 | //ds trigger classic ICP in camera update of landmark coordinates - setup 59 | Vector3 world_coordinates(_world_coordinates); 60 | Matrix3 H(Matrix3::Zero()); 61 | Vector3 b(Vector3::Zero()); 62 | Matrix3 jacobian; 63 | Matrix3 jacobian_transposed; 64 | Matrix3 omega(Matrix3::Identity()); 65 | real total_error_squared_previous = 0; 66 | const real maximum_error_squared_meters = 5*5; 67 | 68 | //ds gauss newton descent 69 | for (uint32_t iteration = 0; iteration < 1000; ++iteration) { 70 | H.setZero(); 71 | b.setZero(); 72 | real total_error_squared = 0; 73 | uint32_t number_of_outliers = 0; 74 | 75 | //ds for each measurement 76 | for (const Measurement& measurement: _measurements) { 77 | omega.setIdentity(); 78 | 79 | //ds sample current state in measurement context 80 | const PointCoordinates camera_coordinates_sampled = measurement.world_to_camera*world_coordinates; 81 | if (camera_coordinates_sampled.z() <= 0) { 82 | ++number_of_outliers; 83 | continue; 84 | } 85 | 86 | //ds compute error 87 | const Vector3 error(camera_coordinates_sampled-measurement.camera_coordinates); 88 | 89 | //ds weight inverse depth 90 | omega *= measurement.inverse_depth_meters; 91 | 92 | //ds update chi 93 | const real error_squared = error.transpose()*omega*error; 94 | total_error_squared += error_squared; 95 | 96 | //ds robust kernel 97 | if (error_squared > maximum_error_squared_meters) { 98 | omega *= maximum_error_squared_meters/error_squared; 99 | ++number_of_outliers; 100 | } 101 | 102 | //ds get the jacobian of the transform part: R 103 | jacobian = measurement.world_to_camera.linear(); 104 | 105 | //ds precompute transposed 106 | jacobian_transposed = jacobian.transpose(); 107 | 108 | //ds accumulate 109 | H += jacobian_transposed*omega*jacobian; 110 | b += jacobian_transposed*omega*error; 111 | } 112 | 113 | //ds update state 114 | world_coordinates += H.fullPivLu().solve(-b); 115 | 116 | //ds check convergence 117 | if (std::fabs(total_error_squared-total_error_squared_previous) < 1e-5 || iteration == 999) { 118 | const uint32_t number_of_inliers = _measurements.size()-number_of_outliers; 119 | 120 | //ds if the number of inliers is higher than the best so far 121 | if (number_of_inliers > _number_of_updates) { 122 | 123 | //ds update landmark state 124 | _world_coordinates = world_coordinates; 125 | _number_of_updates = number_of_inliers; 126 | 127 | //ds if optimization failed and we have less inliers than outliers - reset initial guess 128 | } else if (number_of_inliers < number_of_outliers) { 129 | 130 | //ds reset estimate based on overall average 131 | PointCoordinates world_coordinates_accumulated(PointCoordinates::Zero()); 132 | for (const Measurement& measurement: _measurements) { 133 | world_coordinates_accumulated += measurement.world_coordinates; 134 | } 135 | 136 | //ds set landmark state without increasing update count 137 | _world_coordinates = world_coordinates_accumulated/_measurements.size(); 138 | } 139 | break; 140 | } 141 | 142 | //ds update previous 143 | total_error_squared_previous = total_error_squared; 144 | } 145 | } 146 | 147 | void Landmark::merge(Landmark* landmark_) { 148 | if (landmark_ == this) { 149 | LOG_WARNING(std::cerr << "Landmark::merge|" << _identifier << "|received merge request to itself: " << landmark_ << std::endl) 150 | return; 151 | } 152 | 153 | //ds merge landmark appearances 154 | for (auto& appearance: landmark_->_appearance_map) { 155 | appearance.second->setObjects(this); 156 | } 157 | _appearance_map.insert(landmark_->_appearance_map.begin(), landmark_->_appearance_map.end()); 158 | landmark_->_appearance_map.clear(); 159 | 160 | //ds merge landmark local maps 161 | for (LocalMap* local_map: landmark_->_local_maps) { 162 | local_map->replace(landmark_, this); 163 | } 164 | _local_maps.insert(landmark_->_local_maps.begin(), landmark_->_local_maps.end()); 165 | landmark_->_local_maps.clear(); 166 | 167 | //ds merge descriptors 168 | _descriptors.insert(_descriptors.end(), landmark_->_descriptors.begin(), landmark_->_descriptors.end()); 169 | landmark_->_descriptors.clear(); 170 | 171 | //ds compute new merged world coordinates 172 | _world_coordinates = (_number_of_updates*_world_coordinates+ 173 | landmark_->_number_of_updates*landmark_->_world_coordinates) 174 | /(_number_of_updates+landmark_->_number_of_updates); 175 | 176 | //ds update measurements 177 | _number_of_updates += landmark_->_number_of_updates; 178 | _number_of_recoveries += landmark_->_number_of_recoveries; 179 | _measurements.insert(_measurements.end(), landmark_->_measurements.begin(), landmark_->_measurements.end()); 180 | landmark_->_measurements.clear(); 181 | 182 | //ds connect framepoint history (last update of this with origin of absorbed landmark) 183 | landmark_->_origin->setPrevious(_last_update); 184 | 185 | //ds update track lengths and landmark references until we arrive in the last framepoint of the absorbed landmark 186 | //ds which will replace the _last_update of this landmark 187 | while (_last_update->next()) { 188 | _last_update = _last_update->next(); 189 | _last_update->setLandmark(this); 190 | _last_update->setTrackLength(_last_update->previous()->trackLength()+1); 191 | _last_update->setOrigin(_origin); 192 | } 193 | } 194 | } 195 | -------------------------------------------------------------------------------- /src/types/landmark.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "frame.h" 3 | 4 | namespace proslam { 5 | 6 | //ds friends 7 | class LocalMap; 8 | class WorldMap; 9 | 10 | //ds this class represents a salient 3D point in the world, perceived in a sequence of images 11 | class Landmark { 12 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | //ds exported types 15 | public: 16 | 17 | typedef std::map HBSTMatchableMemoryMap; 18 | 19 | //ds a landmark measurement (used for position optimization) 20 | struct Measurement { 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | Measurement(const FramePoint* framepoint_): world_to_camera(framepoint_->frame()->worldToCameraLeft()), 23 | camera_coordinates(framepoint_->cameraCoordinatesLeft()), 24 | world_coordinates(framepoint_->worldCoordinates()), 25 | inverse_depth_meters(1/framepoint_->cameraCoordinatesLeft().z()) {} 26 | 27 | Measurement(): world_to_camera(TransformMatrix3D::Identity()), 28 | camera_coordinates(PointCoordinates::Zero()), 29 | world_coordinates(PointCoordinates::Zero()), 30 | inverse_depth_meters(0) {} 31 | 32 | TransformMatrix3D world_to_camera; 33 | PointCoordinates camera_coordinates; 34 | PointCoordinates world_coordinates; 35 | real inverse_depth_meters; 36 | }; 37 | 38 | typedef std::vector> MeasurementVector; 39 | 40 | //ds object handling: specific instantiation controlled by WorldMap class (factory) 41 | protected: 42 | 43 | //ds initial landmark coordinates must be provided 44 | Landmark(FramePoint* origin_, const LandmarkParameters* parameters_); 45 | 46 | //ds cleanup of dynamic structures 47 | ~Landmark(); 48 | 49 | //ds prohibit default construction 50 | Landmark() = delete; 51 | 52 | //ds getters/setters 53 | public: 54 | 55 | //ds unique identifier for a landmark (exists once in memory) 56 | inline const Index identifier() const {return _identifier;} 57 | 58 | //ds framepoint in an image at the time when the landmark was created 59 | inline FramePoint* origin() const {return _origin;} 60 | 61 | inline const PointCoordinates& coordinates() const {return _world_coordinates;} 62 | void setCoordinates(const PointCoordinates& coordinates_) {_world_coordinates = coordinates_;} 63 | 64 | //! @brief replaces a matchable in the appearance map 65 | void replace(const HBSTMatchable* matchable_old_, HBSTMatchable* matchable_new_); 66 | 67 | const HBSTMatchableMemoryMap& appearances() const {return _appearance_map;} 68 | 69 | //ds position related 70 | const Count numberOfUpdates() const {return _number_of_updates;} 71 | 72 | //ds information about whether the landmark is visible in the current image 73 | inline const bool isCurrentlyTracked() const {return _is_currently_tracked;} 74 | inline void setIsCurrentlyTracked(const bool& is_currently_tracked_) {_is_currently_tracked = is_currently_tracked_;} 75 | 76 | //ds landmark coordinates update with visual information (tracking) 77 | void update(FramePoint* point_); 78 | 79 | const Count& numberOfRecoveries() const {return _number_of_recoveries;} 80 | void incrementNumberOfRecoveries() {++_number_of_recoveries;} 81 | void setNumberOfRecoveries(const Count& number_of_recoveries_) {_number_of_recoveries = number_of_recoveries_;} 82 | 83 | //! @brief incorporates another landmark into this (e.g. used when relocalizing) 84 | //! @param[in] landmark_ the landmark to absorbed, landmark_ will be freed and its memory location will point to this 85 | void merge(Landmark* landmark_); 86 | 87 | //ds reset allocated object counter 88 | static void reset() {_instances = 0;} 89 | 90 | //ds visualization only 91 | inline const bool isInLoopClosureQuery() const {return _is_in_loop_closure_query;} 92 | inline const bool isInLoopClosureReference() const {return _is_in_loop_closure_reference;} 93 | inline void setIsInLoopClosureQuery(const bool& is_in_loop_closure_query_) {_is_in_loop_closure_query = is_in_loop_closure_query_;} 94 | inline void setIsInLoopClosureReference(const bool& is_in_loop_closure_reference_) {_is_in_loop_closure_reference = is_in_loop_closure_reference_;} 95 | 96 | const std::set& localMaps() const {return _local_maps;} 97 | 98 | //ds attributes 99 | protected: 100 | 101 | //ds unique identifier for a landmark (exists once in memory) 102 | const Identifier _identifier; 103 | 104 | //ds linked FramePoint in an image at the time of creation of this instance and its last update 105 | FramePoint* _origin = nullptr; 106 | FramePoint* _last_update = nullptr; 107 | 108 | //ds world coordinates of the landmark 109 | PointCoordinates _world_coordinates; 110 | 111 | //ds descriptors of this landmark which have not been converted to appearances yet 112 | std::vector _descriptors; 113 | 114 | //ds appearances of this landmark that are captured in a local map (previously contained in _descriptors) 115 | HBSTMatchableMemoryMap _appearance_map; 116 | 117 | //ds connected local maps 118 | std::set _local_maps; 119 | 120 | //ds flags 121 | bool _is_currently_tracked = false; //ds set if the landmark is visible (=tracked) in the current image 122 | 123 | //ds landmark coordinates optimization 124 | MeasurementVector _measurements; 125 | Count _number_of_updates = 0; 126 | Count _number_of_recoveries = 0; 127 | 128 | //ds grant access to landmark factory and helpers 129 | friend WorldMap; 130 | friend LocalMap; 131 | 132 | //ds visualization only 133 | bool _is_in_loop_closure_query = false; 134 | bool _is_in_loop_closure_reference = false; 135 | 136 | //ds class specific 137 | private: 138 | 139 | //! @brief configurable parameters 140 | const LandmarkParameters* _parameters; 141 | 142 | //ds inner instance count - incremented upon constructor call (also unsuccessful calls) 143 | static Count _instances; 144 | 145 | }; 146 | 147 | typedef std::vector LandmarkPointerVector; 148 | typedef std::pair LandmarkPointerMapElement; 149 | typedef std::map LandmarkPointerMap; 150 | typedef std::set LandmarkPointerSet; 151 | 152 | } 153 | -------------------------------------------------------------------------------- /src/types/local_map.cpp: -------------------------------------------------------------------------------- 1 | #include "local_map.h" 2 | 3 | namespace proslam { 4 | 5 | Count LocalMap::_instances = 0; 6 | 7 | LocalMap::LocalMap(FramePointerVector& frames_, 8 | const LocalMapParameters* parameters_, 9 | LocalMap* local_map_root_, 10 | LocalMap* local_map_previous_): _identifier(_instances), 11 | _root(local_map_root_), 12 | _previous(local_map_previous_), 13 | _next(nullptr), 14 | _parameters(parameters_) { 15 | assert(!frames_.empty()); 16 | ++_instances; 17 | 18 | //ds clear structures 19 | clear(); 20 | 21 | //ds if the current local map is preceded by another local map 22 | if (local_map_previous_) { 23 | 24 | //ds link the preceeding local map to this one 25 | _previous->setNext(this); 26 | } 27 | 28 | //ds define keyframe (currently the last frame) 29 | _frames = frames_; 30 | _keyframe = _frames.back(); 31 | _keyframe->setIsKeyframe(true); 32 | 33 | //ds define local map position relative in the world (currently using last frame's pose) 34 | setLocalMapToWorld(_keyframe->robotToWorld(), false); 35 | 36 | //ds keep track of added landmarks in order to add them only once 37 | std::set landmarks_added; 38 | 39 | //ds preallocate bookkeeping with maximum allowed landmarks 40 | const Count maximum_number_of_landmarks = _parameters->maximum_number_of_landmarks; 41 | 42 | //ds create item context for this local map: loop over all frames 43 | for (Frame* frame: frames_) { 44 | const TransformMatrix3D& frame_to_local_map = _world_to_local_map*frame->robotToWorld(); 45 | frame->setLocalMap(this); 46 | frame->setFrameToLocalMap(frame_to_local_map); 47 | 48 | //ds for all framepoints in this frame 49 | for (FramePoint* frame_point: frame->points()) { 50 | 51 | //ds check for landmark 52 | Landmark* landmark = frame_point->landmark(); 53 | 54 | //ds if we have a landmark and it has not been added yet 55 | if (landmark && landmarks_added.count(landmark->identifier()) == 0) { 56 | 57 | //ds create HBST matchables based on available landmark descriptors TODO move this operation into a method of the landmark 58 | HBSTTree::MatchableVector matchables(landmark->_descriptors.size()); 59 | for (Count u = 0; u < matchables.size(); ++u) { 60 | HBSTMatchable* matchable = new HBSTMatchable(landmark, landmark->_descriptors[u], _identifier); 61 | matchables[u] = matchable; 62 | landmark->_appearance_map.insert(std::make_pair(matchable, matchable)); 63 | } 64 | landmark->_descriptors.clear(); 65 | landmark->_local_maps.insert(this); 66 | 67 | //ds create a landmark snapshot and add it to the local map 68 | const PointCoordinates coordinates_in_local_map = _world_to_local_map*landmark->coordinates(); 69 | _landmarks.insert(std::make_pair(landmark->identifier(), LandmarkState(landmark, coordinates_in_local_map))); 70 | 71 | //ds we're only interested in the appearances generated in this local map 72 | _appearances.insert(_appearances.end(), matchables.begin(), matchables.end()); 73 | 74 | //ds block further additions of this landmark 75 | landmarks_added.insert(landmark->identifier()); 76 | } 77 | } 78 | } 79 | 80 | //ds check if we have to sparsify the landmarks TODO implement, check how to trim appearance vector as well 81 | if (_landmarks.size() > maximum_number_of_landmarks) { 82 | LOG_INFO(std::cerr << "LocalMap::LocalMap|" << _identifier 83 | << "|pruning landmarks from: " << landmarks_added.size() << " to: " << maximum_number_of_landmarks << std::endl) 84 | 85 | //ds TODO free unused landmark states 86 | } 87 | 88 | //ds check for low item counts 89 | if (_parameters->minimum_number_of_landmarks > landmarks_added.size()) { 90 | LOG_WARNING(std::cerr << "LocalMap::LocalMap|creating local map with low landmark number: " << landmarks_added.size() << std::endl) 91 | } 92 | } 93 | 94 | LocalMap::~LocalMap() { 95 | clear(); 96 | } 97 | 98 | void LocalMap::clear() { 99 | _landmarks.clear(); 100 | _closures.clear(); 101 | _frames.clear(); 102 | _appearances.clear(); 103 | } 104 | 105 | void LocalMap::update(const TransformMatrix3D& local_map_to_world_) { 106 | setLocalMapToWorld(local_map_to_world_); 107 | 108 | //ds update frame poses for all contained frames 109 | for (Frame* frame: _frames) { 110 | frame->setRobotToWorld(_local_map_to_world*frame->frameToLocalMap()); 111 | } 112 | } 113 | 114 | void LocalMap::replace(Landmark* landmark_old_, Landmark* landmark_new_) { 115 | 116 | //ds remove the old landmark from the local map and check for failure 117 | if (_landmarks.erase(landmark_old_->identifier()) != 1) { 118 | LOG_WARNING(std::cerr << "LocalMap::replace|" << _identifier << "|unable to erase old landmark with ID: " << landmark_old_->identifier() << std::endl) 119 | } 120 | 121 | //ds look if the new landmark is already present (can happen through merging) 122 | LandmarkStateMap::iterator iterator = _landmarks.find(landmark_new_->identifier()); 123 | if (iterator != _landmarks.end()) { 124 | 125 | //ds update the entry 126 | iterator->second.coordinates_in_local_map = _world_to_local_map*landmark_new_->coordinates(); 127 | } else { 128 | 129 | //ds create a new entry with updated landmark coordinates 130 | _landmarks.insert(std::make_pair(landmark_new_->identifier(), LandmarkState(landmark_new_, _world_to_local_map*landmark_new_->coordinates()))); 131 | } 132 | } 133 | 134 | void LocalMap::setLocalMapToWorld(const TransformMatrix3D& local_map_to_world_, const bool update_landmark_world_coordinates_) { 135 | _local_map_to_world = local_map_to_world_; 136 | _world_to_local_map = _local_map_to_world.inverse(); 137 | 138 | //ds update landmark world coordinates according to this local map estimate 139 | if (update_landmark_world_coordinates_) { 140 | for (LandmarkStateMapElement& element: _landmarks) { 141 | element.second.updateCoordinatesInWorld(_local_map_to_world); 142 | } 143 | } 144 | } 145 | } 146 | -------------------------------------------------------------------------------- /src/types/local_map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "landmark.h" 3 | #include "relocalization/closure.h" 4 | 5 | namespace proslam { 6 | 7 | //! @class this class condenses a group of Frame objects into a single Local Map object, which used for relocalization and pose optimization 8 | class LocalMap { 9 | 10 | //ds exported types 11 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | //ds loop closure constraint element between 2 local maps TODO move to relocalizer 14 | struct ClosureConstraint { 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | ClosureConstraint(const LocalMap* local_map_, 17 | const TransformMatrix3D& relation_, 18 | const Closure::CorrespondencePointerVector& landmark_correspondences_, 19 | const real& omega_ = 1): local_map(local_map_), 20 | relation(relation_), 21 | landmark_correspondences(landmark_correspondences_), 22 | omega(omega_){} 23 | 24 | const LocalMap* local_map; 25 | const TransformMatrix3D relation; 26 | const Closure::CorrespondencePointerVector landmark_correspondences; 27 | const real omega; 28 | }; 29 | 30 | typedef std::vector> ClosureConstraintVector; 31 | 32 | //ds landmark snapshot at creation of local map 33 | struct LandmarkState { 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | LandmarkState(Landmark* landmark_, 36 | PointCoordinates coordinates_in_local_map_): landmark(landmark_), 37 | coordinates_in_local_map(coordinates_in_local_map_) {} 38 | 39 | void updateCoordinatesInWorld(const TransformMatrix3D& local_map_to_world_) { 40 | landmark->setCoordinates(local_map_to_world_*coordinates_in_local_map); 41 | } 42 | 43 | Landmark* landmark; 44 | PointCoordinates coordinates_in_local_map; 45 | }; 46 | 47 | typedef std::pair LandmarkStateMapElement; 48 | typedef std::map, Eigen::aligned_allocator > LandmarkStateMap; 49 | 50 | //ds object handling 51 | protected: 52 | 53 | //! @brief constructs a local map that lives in the reference frame of the consumed frames 54 | //! @param[in] frames_ the collection of frames to be contained in the local map (same track) 55 | //! @param[in] local_map_root_ the first local map in the same track 56 | //! @param[in] local_map_previous_ the preceding local map in the same track 57 | //! @param[in] minimum_number_of_landmarks_ target minimum number of landmarks to contain in local map 58 | LocalMap(FramePointerVector& frames_, 59 | const LocalMapParameters* parameters_, 60 | LocalMap* local_map_root_ = nullptr, 61 | LocalMap* local_map_previous_ = nullptr); 62 | 63 | //ds cleanup of dynamic structures 64 | ~LocalMap(); 65 | 66 | //ds prohibit default construction 67 | LocalMap() = delete; 68 | 69 | //ds functionality: 70 | public: 71 | 72 | //! @brief clears all internal structures (prepares a fresh world map) 73 | void clear(); 74 | 75 | //! @brief updates local map pose, automatically updating contained Frame poses (pyramid) 76 | //! @param[in] local_map_to_world_ the local map pose with respect to the world map coordinate frame 77 | void update(const TransformMatrix3D& local_map_to_world_); 78 | 79 | //! @brief adds a loop closure constraint between this local map and a reference map 80 | //! @param[in] local_map_reference_ the corresponding reference local map 81 | //! @param[in] transform_query_to_reference_ the spatial relation between query and reference (from query to reference) 82 | //! @param[in] landmark_correspondences_ underlying landmark correspondences 83 | //! @param[in] omega_ 1D information value of the correspondence 84 | void addCorrespondence(const LocalMap* local_map_reference_, 85 | const TransformMatrix3D& query_to_reference_, 86 | const Closure::CorrespondencePointerVector& landmark_correspondences_, 87 | const real& omega_ = 1) {_closures.push_back(ClosureConstraint(local_map_reference_, query_to_reference_, landmark_correspondences_, omega_));} 88 | 89 | //! @brief replaces a landmark with another (e.g. merged) 90 | //! @param[in] landmark_old_ landmark currently in this local map 91 | //! @param[in] landmark_new_ landmark to replace the currently present landmark_old_ in this local map 92 | void replace(Landmark* landmark_old_, Landmark* landmark_new_); 93 | 94 | //ds getters/setters 95 | public: 96 | 97 | inline const Identifier& identifier() const {return _identifier;} 98 | 99 | inline const TransformMatrix3D& localMapToWorld() const {return _local_map_to_world;} 100 | inline const TransformMatrix3D& worldToLocalMap() const {return _world_to_local_map;} 101 | void setLocalMapToWorld(const TransformMatrix3D& local_map_to_world_, const bool update_landmark_world_coordinates_ = false); 102 | 103 | inline LocalMap* root() {return _root;} 104 | void setRoot(LocalMap* root_) {_root = root_;} 105 | inline LocalMap* previous() {return _previous;} 106 | void setPrevious(LocalMap* local_map_) {_previous = local_map_;} 107 | inline LocalMap* next() {return _next;} 108 | void setNext(LocalMap* local_map_) {_next = local_map_;} 109 | inline Frame* keyframe() const {return _keyframe;} 110 | inline const FramePointerVector& frames() const {return _frames;} 111 | inline LandmarkStateMap& landmarks() {return _landmarks;} 112 | inline AppearanceVector& appearances() {return _appearances;} 113 | inline const AppearanceVector& appearances() const {return _appearances;} 114 | 115 | //ds TODO purge this 116 | inline const ClosureConstraintVector& closures() const {return _closures;} 117 | 118 | //ds reset allocated object counter 119 | static void reset() {_instances = 0;} 120 | 121 | //ds attributes 122 | protected: 123 | 124 | //ds unique identifier for a local map (exists once in memory) 125 | const Identifier _identifier; 126 | 127 | //! @brief pose of the local map with respect to the world map coordinate frame 128 | TransformMatrix3D _local_map_to_world = TransformMatrix3D::Identity(); 129 | 130 | //! @brief transform to map world geometries into the local map coordinate frame 131 | TransformMatrix3D _world_to_local_map = TransformMatrix3D::Identity(); 132 | 133 | //ds links to preceding and subsequent instances 134 | LocalMap* _root; 135 | LocalMap* _previous; 136 | LocalMap* _next; 137 | 138 | //! @brief the keyframe of the local map 139 | Frame* _keyframe; 140 | 141 | //ds the contained Frames 142 | FramePointerVector _frames; 143 | 144 | //ds the contained landmarks with coordinates in the local map frame (i.e. w.r.t. key frame) 145 | //ds these estimates are currently frozen after local map creation TODO update it after optimization 146 | LandmarkStateMap _landmarks; 147 | 148 | //ds appearance vector, corresponding to the union of all appearances stored in _landmarks 149 | //ds this vector is emptied after a local map gets consumed by HBST for place recognition 150 | AppearanceVector _appearances; 151 | 152 | //ds loop closures for the local map 153 | ClosureConstraintVector _closures; 154 | 155 | //ds grant access to local map producer 156 | friend WorldMap; 157 | 158 | //ds class specific 159 | private: 160 | 161 | //! @brief configurable parameters 162 | const LocalMapParameters* _parameters; 163 | 164 | //! @brief inner instance count - incremented upon constructor call (also unsuccessful calls) 165 | static Count _instances; 166 | 167 | }; 168 | 169 | typedef std::vector LocalMapPointerVector; 170 | typedef std::set LocalMapPointerSet; 171 | typedef std::vector ConstLocalMapPointerVector; 172 | 173 | } 174 | -------------------------------------------------------------------------------- /src/types/world_map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "local_map.h" 3 | 4 | namespace proslam { 5 | 6 | //! @class the world map is the overarching map entity, generating and owning all landmarks, frames and local map objects 7 | class WorldMap { 8 | public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | 10 | //ds object handling 11 | public: 12 | 13 | WorldMap(const WorldMapParameters* parameters_); 14 | ~WorldMap(); 15 | 16 | //ds functionality 17 | public: 18 | 19 | //ds clears all internal structures (prepares a fresh world map) 20 | void clear(); 21 | 22 | //! @brief creates a new frame living in this instance at the provided pose 23 | //! @param[in] robot_to_world_ robot to world transform 24 | //! @param[in] timestamp_image_left_seconds_ optional timestamp information 25 | Frame* createFrame(const TransformMatrix3D& robot_to_world_, 26 | const double& timestamp_image_left_seconds_ = 0); 27 | 28 | //! @brief creates a new landmark living in this instance, using the provided framepoint as origin 29 | //! @param[in] origin_ initial framepoint, describing the landmark 30 | Landmark* createLandmark(FramePoint* origin_); 31 | 32 | //ds attempts to create a new local map if the generation criteria are met (returns true if a local map was generated) 33 | const bool createLocalMap(const bool& drop_framepoints_ = false); 34 | 35 | //ds resets the window for the local map generation 36 | void resetWindowForLocalMapCreation(const bool& drop_framepoints_ = false); 37 | 38 | //! @brief adds a loop closure constraint between 2 local maps 39 | //! @param[in] query_ query local map 40 | //! @param[in] reference_ reference local map (fixed, closed against) 41 | //! @param[in] query_to_reference_ query to reference transform 42 | //! @param[in] landmark_correspondences_ landmark correspondences of the loop closure 43 | //! @param[in] information_ information value 44 | void addLoopClosure(LocalMap* query_, 45 | const LocalMap* reference_, 46 | const TransformMatrix3D& query_to_reference_, 47 | const Closure::CorrespondencePointerVector& landmark_correspondences_, 48 | const real& information_ = 1); 49 | 50 | //! @brief dump trajectory to file (in KITTI benchmark format: 4x4 isometries per line and TUM benchmark format: timestamp x z y and qx qy qz qw per line) 51 | //! @param[in] filename_ text file in which the poses are saved to 52 | void writeTrajectoryKITTI(const std::string& filename_ = "") const; 53 | 54 | //! @brief dump trajectory to file (in TUM benchmark format: timestamp x z y and qx qy qz qw per line) 55 | //! @param[in] filename_ text file in which the poses are saved to 56 | void writeTrajectoryTUM(const std::string& filename_ = "") const; 57 | 58 | //! @brief save trajectory to a vector 59 | //! @param[in,out] poses_ vector with poses, set in the function 60 | template 61 | void writeTrajectory(std::vector, Eigen::aligned_allocator>>& poses_) const { 62 | 63 | //ds prepare output vector 64 | poses_.resize(_frames.size()); 65 | 66 | //ds add the pose for each frame 67 | Identifier identifier_frame = 0; 68 | for (const FramePointerMapElement frame: _frames) { 69 | poses_[identifier_frame] = frame.second->robotToWorld().matrix().cast(); 70 | ++identifier_frame; 71 | } 72 | } 73 | 74 | //! @brief save trajectory to a vector with timestamps 75 | //! @param[in,out] poses_ vector with timestamps and poses, set in the function 76 | template 77 | void writeTrajectoryWithTimestamps(std::vector>>& poses_) const { 78 | 79 | //ds prepare output vector 80 | poses_.resize(_frames.size()); 81 | 82 | //ds add the pose for each frame 83 | Identifier identifier_frame = 0; 84 | for (const FramePointerMapElement frame: _frames) { 85 | poses_[identifier_frame].first = frame.second->timestampImageLeftSeconds(); 86 | poses_[identifier_frame].second = frame.second->robotToWorld().matrix().cast(); 87 | ++identifier_frame; 88 | } 89 | } 90 | 91 | //! @brief this function does what you think it does 92 | //! @param[in] frame_ frame at which the track was broken 93 | void breakTrack(Frame* frame_); 94 | 95 | //! @brief this function does what you think it does 96 | //! @param[in] frame_ frame at which the track was found again 97 | void setTrack(Frame* frame_); 98 | 99 | //ds getters/setters 100 | public: 101 | 102 | const Frame* rootFrame() const {return _root_frame;} 103 | Frame* currentFrame() const {return _current_frame;} 104 | void setCurrentFrame(Frame* current_frame_) {_current_frame = current_frame_;} 105 | const Frame* previousFrame() const {return _previous_frame;} 106 | void setPreviousFrame(Frame* previous_frame_) {_previous_frame = previous_frame_;} 107 | 108 | // LandmarkPointerMap& landmarks() {return _landmarks;} 109 | const LandmarkPointerMap& landmarks() const {return _landmarks;} 110 | LandmarkPointerVector& currentlyTrackedLandmarks() {return _currently_tracked_landmarks;} 111 | const LandmarkPointerVector& currentlyTrackedLandmarks() const {return _currently_tracked_landmarks;} 112 | void mergeLandmarks(const LocalMap::ClosureConstraintVector& closures_); 113 | 114 | LocalMap* currentLocalMap() {return _current_local_map;} 115 | const LocalMapPointerVector& localMaps() const {return _local_maps;} 116 | 117 | void setRobotToWorld(const TransformMatrix3D& robot_pose_) {robot_to_world = robot_pose_;} 118 | const TransformMatrix3D robotToWorld() const {return robot_to_world;} 119 | 120 | const bool relocalized() const {return _relocalized;} 121 | const Count& numberOfClosures() const {return _number_of_closures;} 122 | const Count& numberOfMergedLandmarks() const {return _number_of_merged_landmarks;} 123 | 124 | //ds visualization only 125 | const FramePointerMap& frames() const {return _frames;} 126 | const FramePointerVector& frameQueueForLocalMap() const {return _frame_queue_for_local_map;} 127 | void setRobotToWorldGroundTruth(const TransformMatrix3D& robot_to_world_ground_truth_) {if (_current_frame) {_current_frame->setRobotToWorldGroundTruth(robot_to_world_ground_truth_);}} 128 | 129 | const WorldMapParameters* parameters() const {return _parameters;} 130 | 131 | //ds helpers 132 | public: 133 | 134 | //ds obtain angular values from rotation matrix - used for the local map generation criteria in rotation 135 | static const Vector3 toOrientationRodrigues(const Matrix3& rotation_matrix_) { 136 | cv::Vec rotation_angles; 137 | cv::Rodrigues(srrg_core::toCv(rotation_matrix_), rotation_angles); 138 | return srrg_core::fromCv(rotation_angles); 139 | } 140 | 141 | protected: 142 | 143 | //ds robot path information 144 | const Frame* _root_frame = 0; 145 | Frame* _current_frame = 0; 146 | Frame* _previous_frame = 0; 147 | 148 | //ds all permanent landmarks in the map 149 | LandmarkPointerMap _landmarks; 150 | 151 | //ds currently tracked landmarks (=visible in the current image) 152 | LandmarkPointerVector _currently_tracked_landmarks; 153 | 154 | //ds active frames in the map 155 | FramePointerMap _frames; 156 | 157 | //ds localization 158 | TransformMatrix3D robot_to_world = TransformMatrix3D::Identity(); 159 | bool _relocalized = false; 160 | 161 | //ds current frame window buffer for local map generation 162 | real _distance_traveled_window = 0; 163 | real _degrees_rotated_window = 0; 164 | 165 | //ds local map control structures 166 | FramePointerVector _frame_queue_for_local_map; 167 | LocalMap* _current_local_map = nullptr; 168 | LocalMapPointerVector _local_maps; 169 | 170 | //ds track recovery 171 | Frame* _last_frame_before_track_break = nullptr; 172 | LocalMap* _last_local_map_before_track_break = nullptr; 173 | LocalMap* _root_local_map = nullptr; 174 | 175 | //ds informative only 176 | CREATE_CHRONOMETER(landmark_merging) 177 | Count _number_of_merged_landmarks = 0; 178 | 179 | private: 180 | 181 | //! @brief configurable parameters 182 | const WorldMapParameters* _parameters; 183 | 184 | //ds informative/visualization only 185 | Count _number_of_closures = 0; 186 | }; 187 | } 188 | -------------------------------------------------------------------------------- /src/visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg_proslam_visualization_library 2 | image_viewer.cpp 3 | map_viewer.cpp 4 | ) 5 | 6 | target_link_libraries(srrg_proslam_visualization_library 7 | srrg_proslam_types_library 8 | srrg_core_viewers_library 9 | ${OPENGL_gl_LIBRARY} 10 | ${OPENGL_glu_LIBRARY} 11 | ${QGLVIEWER_LIBRARY} 12 | ${SRRG_QT_LIBRARIES} 13 | ) 14 | -------------------------------------------------------------------------------- /src/visualization/image_viewer.cpp: -------------------------------------------------------------------------------- 1 | #include "image_viewer.h" 2 | 3 | #include "types/landmark.h" 4 | 5 | namespace proslam { 6 | 7 | ImageViewer::ImageViewer(ImageViewerParameters* parameters_): _parameters(parameters_), 8 | _current_frame(0) { 9 | LOG_DEBUG(std::cerr << "ImageViewer::ImageViewer|constructed" << std::endl) 10 | } 11 | 12 | ImageViewer::~ImageViewer() { 13 | LOG_DEBUG(std::cerr << "ImageViewer::~ImageViewer|destroyed" << std::endl) 14 | } 15 | 16 | void ImageViewer::configure() { 17 | //ds nothing to do 18 | } 19 | 20 | void ImageViewer::update(const Frame* frame_) { 21 | 22 | //ds start data transfer (this will lock the calling thread if the GUI is busy) - released when the function is quit 23 | std::lock_guard lock(_mutex_data_exchange); 24 | 25 | //ds release previous image 26 | _current_image.release(); 27 | 28 | //ds update handle 29 | _current_frame = frame_; 30 | 31 | //ds set current image 32 | cv::cvtColor(frame_->intensityImageLeft(), _current_image, CV_GRAY2RGB); 33 | 34 | //ds buffer secondary image (if existing and desired) 35 | if (!_current_image_secondary.empty() && 36 | _parameters->tracker_mode == CommandLineParameters::TrackerMode::RGB_DEPTH) { 37 | 38 | //ds upscale depth image to make it more visible 39 | _current_image_secondary = frame_->intensityImageRight().clone()*5; 40 | } 41 | } 42 | 43 | void ImageViewer::draw() { 44 | 45 | //ds lock data transfer while drawing 46 | std::lock_guard lock(_mutex_data_exchange); 47 | 48 | //ds if a valid image is set 49 | if (!_current_image.empty()) { 50 | 51 | //ds draw framepoints 52 | _drawPoints(); 53 | 54 | //ds draw optical flow 55 | _drawTracking(); 56 | 57 | //ds display the image(s) 58 | cv::imshow(_parameters->window_title.c_str(), _current_image); 59 | if (!_current_image_secondary.empty() && 60 | _parameters->tracker_mode == CommandLineParameters::TrackerMode::RGB_DEPTH) { 61 | cv::imshow(_parameters->window_title_secondary.c_str(), _current_image_secondary); 62 | } 63 | cv::waitKey(1); 64 | 65 | // //ds backup images for saving 66 | // _image_to_save = _current_image.clone(); 67 | // _image_to_save_secondary = _current_image_secondary.clone(); 68 | } 69 | } 70 | 71 | void ImageViewer::saveToDisk() { 72 | // if (_current_frame) { 73 | // char buffer_file_name[32]; 74 | // std::snprintf(buffer_file_name, 32, "images/image-rgb-%04lu.jpg", _number_of_saved_images); 75 | // cv::imwrite(buffer_file_name, _image_to_save); 76 | //// std::snprintf(buffer_file_name, 32, "images/image-depth-%04lu.png", _number_of_saved_images); 77 | //// cv::imwrite(buffer_file_name, _image_to_save_secondary); 78 | // ++_number_of_saved_images; 79 | // } 80 | } 81 | 82 | void ImageViewer::_drawPoints() { 83 | if (_current_frame) { 84 | 85 | //ds for all points in the current frame 86 | for (const FramePoint* point: _current_frame->points()) { 87 | 88 | //ds if the point is linked to a landmark 89 | if (point->landmark()) { 90 | cv::Scalar color = CV_COLOR_CODE_BLUE; 91 | 92 | //ds check if the landmark is part of a loop closure 93 | if (point->landmark()->isInLoopClosureQuery() || point->landmark()->isInLoopClosureReference()) { 94 | color = CV_COLOR_CODE_DARKGREEN; 95 | } 96 | 97 | //ds draw the point 98 | cv::circle(_current_image, point->keypointLeft().pt, 2, color, -1); 99 | 100 | //ds draw track length 101 | cv::putText(_current_image, std::to_string(point->trackLength()), point->keypointLeft().pt+cv::Point2f(5, 5), cv::FONT_HERSHEY_SCRIPT_SIMPLEX, 0.25, CV_COLOR_CODE_RED); 102 | } else { 103 | 104 | //ds new point 105 | cv::circle(_current_image, point->keypointLeft().pt, 2, CV_COLOR_CODE_GREEN, -1); 106 | } 107 | } 108 | } 109 | } 110 | 111 | void ImageViewer::_drawTracking() { 112 | if (_current_frame) { 113 | const uint32_t& tracking_distance_pixels = _current_frame->projectionTrackingDistancePixels(); 114 | 115 | //ds for all points in the current frame 116 | for (const FramePoint* point: _current_frame->points()) { 117 | if (point->previous()) { 118 | 119 | //ds for points without landmark, draw a little green dot 120 | if (!point->landmark()) { 121 | cv::circle(_current_image, point->keypointLeft().pt, 2, CV_COLOR_CODE_GREEN, -1); 122 | cv::line(_current_image, point->keypointLeft().pt, point->previous()->keypointLeft().pt, CV_COLOR_CODE_GREEN); 123 | } 124 | 125 | //ds draw projected point and error 126 | cv::circle(_current_image, point->projectionEstimateLeft(), 4, CV_COLOR_CODE_BLUE, 1); 127 | 128 | //ds draw tracking line and circle 129 | cv::circle(_current_image, point->keypointLeft().pt, tracking_distance_pixels, CV_COLOR_CODE_GREEN); 130 | cv::line(_current_image, point->keypointLeft().pt, point->previous()->keypointLeft().pt, CV_COLOR_CODE_GREEN); 131 | } 132 | } 133 | } 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /src/visualization/image_viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "types/frame.h" 4 | 5 | namespace proslam { 6 | 7 | //! @class a simple 2D opencv input image viewer class for processing display 8 | class ImageViewer { 9 | 10 | //ds object life 11 | PROSLAM_MAKE_PROCESSING_CLASS(ImageViewer) 12 | 13 | //ds access 14 | public: 15 | 16 | //! @brief GUI update function, copies framepoints from provided frame into thread-safe, drawable containers - LOCKING 17 | //! @param[in] frame_ the frame to display 18 | void update(const Frame* frame_); 19 | 20 | //! @brief draw function - LOCKING 21 | void draw(); 22 | 23 | //! @brief saves current image to disk 24 | void saveToDisk(); 25 | 26 | //ds helpers 27 | protected: 28 | 29 | //! @brief draws currently generated framepoints with image coordinates 30 | void _drawPoints(); 31 | 32 | //! @brief draws the currently tracked framepoints epipolar lines (< generated) 33 | void _drawTracking(); 34 | 35 | //ds attributes 36 | protected: 37 | 38 | //! @brief mutex for data exchange, owned by the viewer 39 | std::mutex _mutex_data_exchange; 40 | 41 | //! @brief current frame handle (drawn in draw function) 42 | const Frame* _current_frame; 43 | 44 | //! @brief currently displayed image 45 | cv::Mat _current_image; 46 | 47 | //! @brief currently displayed secondary image 48 | cv::Mat _current_image_secondary; 49 | 50 | //! @brief saved images count (for stamp generation) 51 | Count _number_of_saved_images = 0; 52 | 53 | //! @brief last complete images copy to save to disk 54 | cv::Mat _image_to_save; 55 | cv::Mat _image_to_save_secondary; 56 | }; 57 | } 58 | -------------------------------------------------------------------------------- /src/visualization/map_viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include "srrg_core_viewers/simple_viewer.h" 6 | #include "types/world_map.h" 7 | 8 | namespace proslam { 9 | 10 | class MapViewer: public srrg_core_viewers::SimpleViewer { 11 | 12 | //ds object life 13 | PROSLAM_MAKE_PROCESSING_CLASS(MapViewer) 14 | 15 | //ds access 16 | public: 17 | 18 | //! @brief GUI update function, copies framepoints from provided frame into thread-safe, drawable containers - LOCKING 19 | //! @param[in] frame_ the frame to display 20 | void update(const Frame* frame_); 21 | 22 | //! @brief manual data transfer locking (generally used to block the GUI from drawing during critical operations e.g. global map updates) 23 | void lock() {_mutex_data_exchange.lock();} 24 | 25 | //! @brief manual data transfer unlocking (enables the GUI to freely draw again) 26 | void unlock() {_mutex_data_exchange.unlock();} 27 | 28 | //ds setters/getters 29 | public: 30 | 31 | void setWorldMap(const WorldMap* world_map_) {_world_map = world_map_;} 32 | 33 | inline bool landmarksDrawn() const {return _parameters->landmarks_drawn;} 34 | inline void setLandmarksDrawn(const bool& landmarks_drawn_) {_parameters->landmarks_drawn = landmarks_drawn_;} 35 | 36 | inline bool framesDrawn() const {return _parameters->frames_drawn;} 37 | inline void setFramesDrawn(const bool& frames_drawn_) {_parameters->frames_drawn = frames_drawn_;} 38 | 39 | inline bool followRobot() const {return _parameters->follow_robot;} 40 | inline void setFollowRobot(const bool& follow_robot_) {_parameters->follow_robot = follow_robot_;} 41 | 42 | void setWorldToRobotOrigin(const TransformMatrix3D& world_to_robot_origin_) {_world_to_robot_origin = world_to_robot_origin_;} 43 | void setRotationRobotView(const TransformMatrix3D& rotation_robot_view_) {_robot_viewpoint = rotation_robot_view_;} 44 | void setCameraLeftToRobot(const TransformMatrix3D& camera_left_to_robot_) {_camera_left_to_robot = camera_left_to_robot_;} 45 | 46 | inline const bool optionStepwisePlayback() const {return _option_stepwise_playback;} 47 | inline const Count requestedPlaybackSteps() const {return _requested_playback_steps;} 48 | inline void decrementPlaybackSteps() {assert(_requested_playback_steps > 0); --_requested_playback_steps;} 49 | 50 | //ds helpers 51 | protected: 52 | 53 | //! @brief Qt standard draw function - LOCKING 54 | virtual void draw(); 55 | 56 | //! @brief Qt key event handling 57 | virtual void keyPressEvent(QKeyEvent* event_); 58 | 59 | //! @brief Qt help string 60 | virtual QString helpString() const; 61 | 62 | //! @brief draws frame in the map, differentiating between local map anchors and regular frames 63 | //! @param[in] frame_ 64 | //! @param[in] color_rgb_ 65 | void _drawFrame(const Frame* frame_, const Vector3& color_rgb_) const; 66 | 67 | //! @brief draws landmark in different states in the map 68 | void _drawLandmarks() const; 69 | 70 | //! @brief draws framepoints for the currently active frame 71 | void _drawFramepoints() const; 72 | 73 | //ds attributes 74 | protected: 75 | 76 | //! @brief mutex for data exchange, owned by the viewer 77 | std::mutex _mutex_data_exchange; 78 | 79 | //! @brief map context (drawn with draw and updated with update method) 80 | const WorldMap* _world_map; 81 | 82 | //! @brief current frame handle (drawn in draw function) 83 | const Frame* _current_frame; 84 | 85 | //! @brief enable stepwise playback 86 | std::atomic _option_stepwise_playback; 87 | 88 | //! @brief stepping buffer for stepwise playback 89 | std::atomic _requested_playback_steps; 90 | 91 | //! @brief configuration: camera left to robot relation 92 | TransformMatrix3D _camera_left_to_robot; 93 | 94 | //! @brief display transformation: origin viewpoint 95 | TransformMatrix3D _world_to_robot_origin; 96 | 97 | //! @brief display transformation: viewpoint adjustment for ego perspective 98 | TransformMatrix3D _robot_viewpoint; 99 | 100 | //! @brief current robot position 101 | TransformMatrix3D _world_to_robot; 102 | }; 103 | } 104 | --------------------------------------------------------------------------------